Skip to content

Commit c433ec6

Browse files
rtlopezrtlopez
committed
inner + outer pid terms api
1 parent 222cd86 commit c433ec6

File tree

4 files changed

+128
-0
lines changed

4 files changed

+128
-0
lines changed

‎lib/Espfc/src/Connect/EspProtocol.hpp‎

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,8 @@ enum EspCommand : uint8_t
1919
ESP_CMD_GPS = 0x0a,
2020
ESP_CMD_GPS_INFO = 0x0b,
2121
ESP_CMD_RPM_TLM = 0x0c,
22+
ESP_CMD_PID_INNER = 0x0d,
23+
ESP_CMD_PID_OUTER = 0x0e,
2224
ESP_CMD_DEBUG = 0x0f,
2325

2426
ESP_CMD_MODE_NAMES = 0x10,
@@ -170,6 +172,27 @@ struct EspCmdSensors
170172
int16_t baroAlt;
171173
} __attribute__((packed));
172174

175+
struct EspCmdPidTerm {
176+
int16_t p;
177+
int16_t i;
178+
int16_t d;
179+
int16_t f;
180+
} __attribute__((packed));
181+
182+
struct EspCmdPidInner
183+
{
184+
EspCmdPidTerm roll;
185+
EspCmdPidTerm pitch;
186+
EspCmdPidTerm yaw;
187+
EspCmdPidTerm alt;
188+
} __attribute__((packed));
189+
190+
struct EspCmdPidOuter
191+
{
192+
EspCmdPidTerm roll;
193+
EspCmdPidTerm pitch;
194+
} __attribute__((packed));
195+
173196
struct EspCmdInput
174197
{
175198
uint8_t channelCount; // number of input channels

‎lib/Espfc/src/Connect/MspProcessor.cpp‎

Lines changed: 54 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -479,6 +479,60 @@ void MspProcessor::processCommandESP(MspMessage& m, MspResponse& r, Device::Seri
479479
}
480480
break;
481481

482+
case ESP_CMD_PID_INNER:
483+
{
484+
const auto& inner = _model.state.innerPid;
485+
EspCmdPidInner res = {
486+
.roll = {
487+
.p = Utils::fcast<int16_t>(inner[AXIS_ROLL].pTerm * 1000.0f),
488+
.i = Utils::fcast<int16_t>(inner[AXIS_ROLL].iTerm * 1000.0f),
489+
.d = Utils::fcast<int16_t>(inner[AXIS_ROLL].dTerm * 1000.0f),
490+
.f = Utils::fcast<int16_t>(inner[AXIS_ROLL].fTerm * 1000.0f),
491+
},
492+
.pitch = {
493+
.p = Utils::fcast<int16_t>(inner[AXIS_PITCH].pTerm * 1000.0f),
494+
.i = Utils::fcast<int16_t>(inner[AXIS_PITCH].iTerm * 1000.0f),
495+
.d = Utils::fcast<int16_t>(inner[AXIS_PITCH].dTerm * 1000.0f),
496+
.f = Utils::fcast<int16_t>(inner[AXIS_PITCH].fTerm * 1000.0f),
497+
},
498+
.yaw = {
499+
.p = Utils::fcast<int16_t>(inner[AXIS_YAW].pTerm * 1000.0f),
500+
.i = Utils::fcast<int16_t>(inner[AXIS_YAW].iTerm * 1000.0f),
501+
.d = Utils::fcast<int16_t>(inner[AXIS_YAW].dTerm * 1000.0f),
502+
.f = Utils::fcast<int16_t>(inner[AXIS_YAW].fTerm * 1000.0f),
503+
},
504+
.alt = {
505+
.p = Utils::fcast<int16_t>(inner[AXIS_THRUST].pTerm * 1000.0f),
506+
.i = Utils::fcast<int16_t>(inner[AXIS_THRUST].iTerm * 1000.0f),
507+
.d = Utils::fcast<int16_t>(inner[AXIS_THRUST].dTerm * 1000.0f),
508+
.f = Utils::fcast<int16_t>(inner[AXIS_THRUST].fTerm * 1000.0f),
509+
}
510+
};
511+
r.write(res);
512+
}
513+
break;
514+
515+
case ESP_CMD_PID_OUTER:
516+
{
517+
const auto& outer = _model.state.outerPid;
518+
EspCmdPidOuter res{
519+
.roll = {
520+
.p = Utils::fcast<int16_t>(outer[AXIS_ROLL].pTerm * 1000.0f),
521+
.i = Utils::fcast<int16_t>(outer[AXIS_ROLL].iTerm * 1000.0f),
522+
.d = Utils::fcast<int16_t>(outer[AXIS_ROLL].dTerm * 1000.0f),
523+
.f = Utils::fcast<int16_t>(outer[AXIS_ROLL].fTerm * 1000.0f),
524+
},
525+
.pitch = {
526+
.p = Utils::fcast<int16_t>(outer[AXIS_PITCH].pTerm * 1000.0f),
527+
.i = Utils::fcast<int16_t>(outer[AXIS_PITCH].iTerm * 1000.0f),
528+
.d = Utils::fcast<int16_t>(outer[AXIS_PITCH].dTerm * 1000.0f),
529+
.f = Utils::fcast<int16_t>(outer[AXIS_PITCH].fTerm * 1000.0f),
530+
},
531+
};
532+
r.write(res);
533+
}
534+
break;
535+
482536
case ESP_CMD_DEBUG:
483537
{
484538
EspCmdDebug debug = {};

‎lib/Espfc/src/Utils/Math.hpp‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@
44
#include <cstddef>
55
#include <cmath>
66
#include <algorithm>
7+
#include <limits>
78

89
namespace Espfc {
910

@@ -53,6 +54,15 @@ class Peak
5354
return value;
5455
}
5556

57+
template<typename T>
58+
T fcast(const float value)
59+
{
60+
long v = lrintf(value);
61+
if (v >= std::numeric_limits<T>::max()) return std::numeric_limits<T>::max();
62+
else if (v <= std::numeric_limits<T>::min()) return std::numeric_limits<T>::min();
63+
return static_cast<T>(v);
64+
}
65+
5666
inline int alignToClock(uint32_t clock, uint32_t maxFreq)
5767
{
5868
uint32_t result = clock;

‎test/test_math/test_math.cpp‎

Lines changed: 41 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -41,6 +41,46 @@ void test_math_map3()
4141
TEST_ASSERT_FLOAT_WITHIN(1.f, -500.f, Utils::map3(-50.0f, -100.0f, 0.0f, 100.0f, -1000.0f, 0.0f, 1000.0f));
4242
}
4343

44+
void test_math_fcast()
45+
{
46+
TEST_ASSERT_EQUAL_INT32( 0, Utils::fcast<int8_t>(0.0f));
47+
TEST_ASSERT_EQUAL_INT32( 0, Utils::fcast<int8_t>(0.1f));
48+
TEST_ASSERT_EQUAL_INT32( 0, Utils::fcast<int8_t>(0.5f));
49+
TEST_ASSERT_EQUAL_INT32( 1, Utils::fcast<int8_t>(0.501f));
50+
TEST_ASSERT_EQUAL_INT32( 0, Utils::fcast<int8_t>(-0.5f));
51+
TEST_ASSERT_EQUAL_INT32( -1, Utils::fcast<int8_t>(-0.51f));
52+
TEST_ASSERT_EQUAL_INT32( 127, Utils::fcast<int8_t>(127.0f));
53+
TEST_ASSERT_EQUAL_INT32( -128, Utils::fcast<int8_t>(-128.0f));
54+
TEST_ASSERT_EQUAL_INT32( 127, Utils::fcast<int8_t>(32767.0f));
55+
TEST_ASSERT_EQUAL_INT32( -128, Utils::fcast<int8_t>(-32768.0f));
56+
TEST_ASSERT_EQUAL_INT32( 127, Utils::fcast<int8_t>(50000.0f));
57+
TEST_ASSERT_EQUAL_INT32( -128, Utils::fcast<int8_t>(-50000.0f));
58+
59+
TEST_ASSERT_EQUAL_INT32( 0, Utils::fcast<uint8_t>(0.0f));
60+
TEST_ASSERT_EQUAL_INT32( 0, Utils::fcast<uint8_t>(0.5f));
61+
TEST_ASSERT_EQUAL_INT32( 1, Utils::fcast<uint8_t>(0.501f));
62+
TEST_ASSERT_EQUAL_INT32( 0, Utils::fcast<uint8_t>(-0.5f));
63+
TEST_ASSERT_EQUAL_INT32( 0, Utils::fcast<uint8_t>(-0.51f));
64+
TEST_ASSERT_EQUAL_INT32( 127, Utils::fcast<uint8_t>(127.0f));
65+
TEST_ASSERT_EQUAL_INT32( 0, Utils::fcast<uint8_t>(-128.0f));
66+
TEST_ASSERT_EQUAL_INT32( 255, Utils::fcast<uint8_t>(32767.0f));
67+
TEST_ASSERT_EQUAL_INT32( 0, Utils::fcast<uint8_t>(-32768.0f));
68+
TEST_ASSERT_EQUAL_INT32( 255, Utils::fcast<uint8_t>(50000.0f));
69+
TEST_ASSERT_EQUAL_INT32( 0, Utils::fcast<uint8_t>(-50000.0f));
70+
71+
TEST_ASSERT_EQUAL_INT32( 0, Utils::fcast<int16_t>(0.0f));
72+
TEST_ASSERT_EQUAL_INT32( 0, Utils::fcast<int16_t>(0.5f));
73+
TEST_ASSERT_EQUAL_INT32( 1, Utils::fcast<int16_t>(0.501f));
74+
TEST_ASSERT_EQUAL_INT32( 0, Utils::fcast<int16_t>(-0.5f));
75+
TEST_ASSERT_EQUAL_INT32( -1, Utils::fcast<int16_t>(-0.6f));
76+
TEST_ASSERT_EQUAL_INT32( 127, Utils::fcast<int16_t>(127.0f));
77+
TEST_ASSERT_EQUAL_INT32( -128, Utils::fcast<int16_t>(-128.0f));
78+
TEST_ASSERT_EQUAL_INT32( 32767, Utils::fcast<int16_t>(32767.0f));
79+
TEST_ASSERT_EQUAL_INT32(-32768, Utils::fcast<int16_t>(-32768.0f));
80+
TEST_ASSERT_EQUAL_INT32( 32767, Utils::fcast<int16_t>(50000.0f));
81+
TEST_ASSERT_EQUAL_INT32(-32768, Utils::fcast<int16_t>(-50000.0f));
82+
}
83+
4484
void test_math_baro_altitude()
4585
{
4686
TEST_ASSERT_FLOAT_WITHIN(0.1f, 0.0f, Utils::toAltitude(101325.f)); // sea level
@@ -1304,6 +1344,7 @@ int main(int argc, char **argv)
13041344
RUN_TEST(test_math_map);
13051345
RUN_TEST(test_math_map3);
13061346
RUN_TEST(test_math_deadband);
1347+
RUN_TEST(test_math_fcast);
13071348

13081349
RUN_TEST(test_math_bit);
13091350
RUN_TEST(test_math_bitmask);

0 commit comments

Comments
 (0)