Changes from review and add unit tests

Stylistic updates and add tests to the PID unit tests to verify launch control behavior

src/main/cms/cms_menu_imu.c

@@ -246,8 +246,8 @@ static CMS_Menu cmsx_menuRateProfile = {
 
 #ifdef USE_LAUNCH_CONTROL
 static uint8_t cmsx_launchControlMode;
-static uint8_t cmsx_launchControlTriggerMode;
+static uint8_t cmsx_launchControlAllowTriggerReset;
-static uint8_t cmsx_launchControlThrottlePct;
+static uint8_t cmsx_launchControlThrottlePercent;
 static uint8_t cmsx_launchControlAngleLimit;
 static uint8_t cmsx_launchControlGain;
 
@@ -256,8 +256,8 @@ static long cmsx_launchControlOnEnter(void)
     const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
     
     cmsx_launchControlMode  = pidProfile->launchControlMode;
-    cmsx_launchControlTriggerMode  = pidProfile->launchControlTriggerMode;
+    cmsx_launchControlAllowTriggerReset  = pidProfile->launchControlAllowTriggerReset;
-    cmsx_launchControlThrottlePct  = pidProfile->launchControlThrottlePct;
+    cmsx_launchControlThrottlePercent  = pidProfile->launchControlThrottlePercent;
     cmsx_launchControlAngleLimit  = pidProfile->launchControlAngleLimit;
     cmsx_launchControlGain  = pidProfile->launchControlGain;
 
@@ -271,8 +271,8 @@ static long cmsx_launchControlOnExit(const OSD_Entry *self)
     pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex);
 
     pidProfile->launchControlMode = cmsx_launchControlMode;
-    pidProfile->launchControlTriggerMode = cmsx_launchControlTriggerMode;
+    pidProfile->launchControlAllowTriggerReset = cmsx_launchControlAllowTriggerReset;
-    pidProfile->launchControlThrottlePct = cmsx_launchControlThrottlePct;
+    pidProfile->launchControlThrottlePercent = cmsx_launchControlThrottlePercent;
     pidProfile->launchControlAngleLimit = cmsx_launchControlAngleLimit;
     pidProfile->launchControlGain = cmsx_launchControlGain;
 
@@ -283,10 +283,10 @@ static OSD_Entry cmsx_menuLaunchControlEntries[] = {
     { "-- LAUNCH CONTROL --", OME_Label, NULL, pidProfileIndexString, 0 },
 
     { "MODE",             OME_TAB,   NULL, &(OSD_TAB_t)   { &cmsx_launchControlMode, LAUNCH_CONTROL_MODE_COUNT - 1, osdLaunchControlModeNames}, 0 },
-    { "TRIGGER MODE",     OME_TAB,   NULL, &(OSD_TAB_t)   { &cmsx_launchControlTriggerMode, LAUNCH_CONTROL_TRIGGER_MODE_COUNT - 1, osdLaunchControlTriggerModeNames}, 0 },
+    { "ALLOW RESET",      OME_Bool,  NULL, &cmsx_launchControlAllowTriggerReset, 0 },
-    { "TRIGGER THROTTLE", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_launchControlThrottlePct, 0,  50, 1 } , 0 },
+    { "TRIGGER THROTTLE", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_launchControlThrottlePercent, 0,  50, 1 } , 0 },
-    { "ANGLE LIMIT",      OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_launchControlAngleLimit,  0,  80, 1 } , 0 },
+    { "ANGLE LIMIT",      OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_launchControlAngleLimit,      0,  80, 1 } , 0 },
-    { "ITERM GAIN",       OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_launchControlGain,        0, 200, 1 } , 0 },
+    { "ITERM GAIN",       OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_launchControlGain,            0, 200, 1 } , 0 },
     { "BACK", OME_Back, NULL, NULL, 0 },
     { NULL, OME_END, NULL, NULL, 0 }
 };

src/main/fc/core.c

@@ -102,7 +102,7 @@ enum {
     ARMING_DELAYED_DISARMED = 0,
     ARMING_DELAYED_NORMAL = 1,
     ARMING_DELAYED_CRASHFLIP = 2,
-    ARMING_DELAYED_LAUNCH_CONTROL = 3
+    ARMING_DELAYED_LAUNCH_CONTROL = 3,
 };
 
 #define GYRO_WATCHDOG_DELAY 80 //  delay for gyro sync
@@ -154,10 +154,6 @@ const char * const osdLaunchControlModeNames[] = {
     "PITCHONLY",
     "FULL"
 };
-const char * const osdLaunchControlTriggerModeNames[] = {
-    "MULTIPLE",
-    "SINGLE"
-};
 #endif
 
 PG_REGISTER_WITH_RESET_TEMPLATE(throttleCorrectionConfig_t, throttleCorrectionConfig, PG_THROTTLE_CORRECTION_CONFIG, 0);
@@ -197,11 +193,9 @@ bool canUseLaunchControl(void)
         && (!featureIsEnabled(FEATURE_MOTOR_STOP) || airmodeIsEnabled())  // can't use when motors are stopped
         && !featureIsEnabled(FEATURE_3D) // pitch control is not 3D aware
         && (flightModeFlags == 0)) {     // don't want to use unless in acro mode
-        
         return true;
-    } else {
-        return false;
     }
+    return false;
 }
 #endif
 
@@ -714,7 +708,7 @@ bool processRx(timeUs_t currentTimeUs)
 
 #ifdef USE_LAUNCH_CONTROL
     if (ARMING_FLAG(ARMED)) {
-        if (launchControlActive && (throttlePercent > currentPidProfile->launchControlThrottlePct)) {
+        if (launchControlActive && (throttlePercent > currentPidProfile->launchControlThrottlePercent)) {
             // throttle limit trigger reached, launch triggered
             // reset the iterms as they may be at high values from holding the launch position
             launchControlState = LAUNCH_CONTROL_TRIGGERED;
@@ -726,7 +720,7 @@ bool processRx(timeUs_t currentTimeUs)
             // and the mode switch is turned off.
             // For trigger mode SINGLE we never reset the state and only a single
             // launch is allowed until a reboot.
-            if ((currentPidProfile->launchControlTriggerMode == LAUNCH_CONTROL_TRIGGER_MODE_MULTIPLE) && !IS_RC_MODE_ACTIVE(BOXLAUNCHCONTROL)) {
+            if (currentPidProfile->launchControlAllowTriggerReset && !IS_RC_MODE_ACTIVE(BOXLAUNCHCONTROL)) {
                 launchControlState = LAUNCH_CONTROL_DISABLED;
             }
         } else {

src/main/fc/core.h

@@ -47,15 +47,8 @@ typedef enum {
     LAUNCH_CONTROL_MODE_COUNT // must be the last element
 } launchControlMode_e;
 
-typedef enum {
-    LAUNCH_CONTROL_TRIGGER_MODE_MULTIPLE = 0,
-    LAUNCH_CONTROL_TRIGGER_MODE_SINGLE,
-    LAUNCH_CONTROL_TRIGGER_MODE_COUNT  // must be the last element
-} launchControlTriggerMode_e;
-
 #ifdef USE_LAUNCH_CONTROL
 extern const char * const osdLaunchControlModeNames[LAUNCH_CONTROL_MODE_COUNT];
-extern const char * const osdLaunchControlTriggerModeNames[LAUNCH_CONTROL_TRIGGER_MODE_COUNT];
 #endif
 
 PG_DECLARE(throttleCorrectionConfig_t, throttleCorrectionConfig);

src/main/flight/pid.c

@@ -169,10 +169,10 @@ void resetPidProfile(pidProfile_t *pidProfile)
         .dterm_filter_type = FILTER_PT1,
         .dterm_filter2_type = FILTER_PT1,
         .launchControlMode = LAUNCH_CONTROL_MODE_NORMAL,
-        .launchControlThrottlePct = 20,
+        .launchControlThrottlePercent = 20,
         .launchControlAngleLimit = 0,
         .launchControlGain = 40,
-        .launchControlTriggerMode = LAUNCH_CONTROL_TRIGGER_MODE_MULTIPLE,
+        .launchControlAllowTriggerReset = true,
     );
 #ifdef USE_DYN_LPF
     pidProfile->dterm_lowpass_hz = 120;
@@ -1019,7 +1019,7 @@ static float applyLaunchControl(int axis, const rollAndPitchTrims_t *angleTrim)
 
     // If ACC is enabled and a limit angle is set, then try to limit forward tilt
     // to that angle and slow down the rate as the limit is approached to reduce overshoot
-    if ((axis == FD_PITCH) && (launchControlAngleLimit > 0) && (ret > 0.0f)) {
+    if ((axis == FD_PITCH) && (launchControlAngleLimit > 0) && (ret > 0)) {
         const float currentAngle = (attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f;
         if (currentAngle >= launchControlAngleLimit) {
             ret = 0.0f;

src/main/flight/pid.h

@@ -152,10 +152,10 @@ typedef struct pidProfile_s {
     uint16_t dyn_lpf_dterm_max_hz;
     uint8_t  dyn_lpf_dterm_idle;
     uint8_t launchControlMode;              // Whether launch control is limited to pitch only (launch stand or top-mount) or all axes (on battery)
-    uint8_t launchControlThrottlePct;       // Throttle percentage to trigger launch for launch control
+    uint8_t launchControlThrottlePercent;   // Throttle percentage to trigger launch for launch control
     uint8_t launchControlAngleLimit;        // Optional launch control angle limit (requires ACC)
     uint8_t launchControlGain;              // Iterm gain used while launch control is active
-    uint8_t launchControlTriggerMode;       // Controls trigger behavior and whether the trigger can be reset
+    uint8_t launchControlAllowTriggerReset; // Controls trigger behavior and whether the trigger can be reset
 } pidProfile_t;
 
 PG_DECLARE_ARRAY(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles);

src/main/interface/settings.c

@@ -405,9 +405,6 @@ static const char * const lookupTableSdcardMode[] = {
 static const char * const lookupTableLaunchControlMode[] = {
     "NORMAL", "PITCHONLY", "FULL"
 };
-static const char * const lookupTableLaunchControlTriggerMode[] = {
-    "MULTIPLE", "SINGLE"
-};
 #endif
 
 #define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) }
@@ -512,7 +509,6 @@ const lookupTableEntry_t lookupTables[] = {
 #endif
 #ifdef USE_LAUNCH_CONTROL
     LOOKUP_TABLE_ENTRY(lookupTableLaunchControlMode),
-    LOOKUP_TABLE_ENTRY(lookupTableLaunchControlTriggerMode),
 #endif
 };
 
@@ -927,8 +923,8 @@ const clivalue_t valueTable[] = {
 
 #ifdef USE_LAUNCH_CONTROL
     { "launch_control_mode",        VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LAUNCH_CONTROL_MODE }, PG_PID_PROFILE, offsetof(pidProfile_t, launchControlMode) },
-    { "launch_trigger_mode",        VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LAUNCH_CONTROL_TRIGGER_MODE }, PG_PID_PROFILE, offsetof(pidProfile_t, launchControlTriggerMode) },
+    { "launch_trigger_allow_reset", VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, launchControlAllowTriggerReset) },
-    { "launch_trigger_throttle_percent", VAR_UINT8 | PROFILE_VALUE,  .config.minmax = { 0, 50 }, PG_PID_PROFILE, offsetof(pidProfile_t, launchControlThrottlePct) },
+    { "launch_trigger_throttle_percent", VAR_UINT8 | PROFILE_VALUE,  .config.minmax = { 0, 50 }, PG_PID_PROFILE, offsetof(pidProfile_t, launchControlThrottlePercent) },
     { "launch_angle_limit",         VAR_UINT8 | PROFILE_VALUE,  .config.minmax = { 0, 80 }, PG_PID_PROFILE, offsetof(pidProfile_t, launchControlAngleLimit) },
     { "launch_control_gain",        VAR_UINT8 | PROFILE_VALUE,  .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, launchControlGain) },
 #endif

src/main/interface/settings.h

@@ -125,7 +125,6 @@ typedef enum {
 #endif
 #ifdef USE_LAUNCH_CONTROL
     TABLE_LAUNCH_CONTROL_MODE,
-    TABLE_LAUNCH_CONTROL_TRIGGER_MODE,
 #endif
     LOOKUP_TABLE_COUNT
 } lookupTableIndex_e;

src/test/Makefile

@@ -325,7 +325,8 @@ pid_unittest_SRC :=  \
 pid_unittest_DEFINES := \
 		USE_ITERM_RELAX \
 		USE_RC_SMOOTHING_FILTER \
-		USE_ABSOLUTE_CONTROL
+		USE_ABSOLUTE_CONTROL\
+		USE_LAUNCH_CONTROL
 
 rcdevice_unittest_DEFINES := \
 		USE_RCDEVICE

src/test/unit/pid_unittest.cc

@@ -64,6 +64,9 @@ extern "C" {
     gyro_t gyro;
     attitudeEulerAngles_t attitude;
 
+    bool unitLaunchControlActive = false;
+    launchControlMode_e unitLaunchControlMode = LAUNCH_CONTROL_MODE_NORMAL;
+
     float getThrottlePIDAttenuation(void) { return simulatedThrottlePIDAttenuation; }
     float getMotorMixRange(void) { return simulatedMotorMixRange; }
     float getSetpointRate(int axis) { return simulatedSetpointRate[axis]; }
@@ -73,7 +76,7 @@ extern "C" {
     bool gyroOverflowDetected(void) { return false; }
     float getRcDeflection(int axis) { return simulatedRcDeflection[axis]; }
     void beeperConfirmationBeeps(uint8_t) { }
-    bool isLaunchControlActive() {return false; }
+    bool isLaunchControlActive(void) {return unitLaunchControlActive; }
 }
 
 pidProfile_t *pidProfile;
@@ -128,6 +131,8 @@ void setDefaultTestSettings(void) {
     pidProfile->iterm_relax_cutoff = 11,
     pidProfile->iterm_relax_type = ITERM_RELAX_SETPOINT,
     pidProfile->abs_control_gain = 0,
+    pidProfile->launchControlMode = LAUNCH_CONTROL_MODE_NORMAL,
+    pidProfile->launchControlGain = 40,
 
     gyro.targetLooptime = 4000;
 }
@@ -160,6 +165,8 @@ void resetTest(void) {
     attitude.values.yaw = 0;
 
     flightModeFlags = 0;
+    unitLaunchControlActive = false;
+    pidProfile->launchControlMode = unitLaunchControlMode;
     pidInit(pidProfile);
 
     // Run pidloop for a while after reset
@@ -653,3 +660,118 @@ TEST(pidControllerTest, testItermRotationHandling) {
     ASSERT_NEAR(860.37, pidData[FD_PITCH].I, calculateTolerance(860.37));
     ASSERT_NEAR(1139.6, pidData[FD_YAW].I, calculateTolerance(1139.6));
 }
+
+TEST(pidControllerTest, testLaunchControl) {
+    // The launchControlGain is indirectly tested since when launch control is active the
+    // the gain overrides the PID settings. If the logic to use launchControlGain wasn't
+    // working then any I calculations would be incorrect.
+
+    resetTest();
+    unitLaunchControlActive = true;
+    ENABLE_ARMING_FLAG(ARMED);
+    pidStabilisationState(PID_STABILISATION_ON);
+
+    // test that feedforward and D are disabled (always zero) when launch control is active
+    // set initial state
+    pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
+
+    EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].F);
+    EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].F);
+    EXPECT_FLOAT_EQ(0, pidData[FD_YAW].F);
+    EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D);
+    EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D);
+    EXPECT_FLOAT_EQ(0, pidData[FD_YAW].D);
+
+    // Move the sticks to induce feedforward
+    setStickPosition(FD_ROLL, 0.5f);
+    setStickPosition(FD_PITCH, -0.5f);
+    setStickPosition(FD_YAW, -0.5f);
+
+    // add gyro activity to induce D
+    gyro.gyroADCf[FD_ROLL] = -1000;
+    gyro.gyroADCf[FD_PITCH] = 1000;
+    gyro.gyroADCf[FD_YAW] = -1000;
+
+    pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
+
+    // validate that feedforwad is still 0
+    EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].F);
+    EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].F);
+    EXPECT_FLOAT_EQ(0, pidData[FD_YAW].F);
+
+    // validate that D is still 0
+    EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].D);
+    EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].D);
+    EXPECT_FLOAT_EQ(0, pidData[FD_YAW].D);
+
+    // test NORMAL mode - expect P/I on roll and pitch, P on yaw but I == 0
+    unitLaunchControlMode = LAUNCH_CONTROL_MODE_NORMAL;
+    resetTest();
+    unitLaunchControlActive = true;
+    ENABLE_ARMING_FLAG(ARMED);
+    pidStabilisationState(PID_STABILISATION_ON);
+
+    pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
+
+    gyro.gyroADCf[FD_ROLL] = -20;
+    gyro.gyroADCf[FD_PITCH] = 20;
+    gyro.gyroADCf[FD_YAW] = -20;
+    pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
+
+    ASSERT_NEAR(25.62,  pidData[FD_ROLL].P,  calculateTolerance(25.62));
+    ASSERT_NEAR(1.56,   pidData[FD_ROLL].I,  calculateTolerance(1.56));
+    ASSERT_NEAR(-37.15, pidData[FD_PITCH].P, calculateTolerance(-37.15));
+    ASSERT_NEAR(-1.56,  pidData[FD_PITCH].I, calculateTolerance(-1.56));
+    ASSERT_NEAR(44.84,  pidData[FD_YAW].P,   calculateTolerance(44.84));
+    EXPECT_FLOAT_EQ(0, pidData[FD_YAW].I);
+
+    // test PITCHONLY mode - expect P/I only on pitch; I cannot go negative
+    unitLaunchControlMode = LAUNCH_CONTROL_MODE_PITCHONLY;
+    resetTest();
+    unitLaunchControlActive = true;
+    ENABLE_ARMING_FLAG(ARMED);
+    pidStabilisationState(PID_STABILISATION_ON);
+
+    pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
+
+    // first test that pitch I is prevented from going negative
+    gyro.gyroADCf[FD_ROLL] = 0;
+    gyro.gyroADCf[FD_PITCH] = 20;
+    gyro.gyroADCf[FD_YAW] = 0;
+    pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
+
+    EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].I);
+
+    gyro.gyroADCf[FD_ROLL] = 20;
+    gyro.gyroADCf[FD_PITCH] = -20;
+    gyro.gyroADCf[FD_YAW] = 20;
+    pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
+
+    EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].P);
+    EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].I);
+    ASSERT_NEAR(37.15, pidData[FD_PITCH].P, calculateTolerance(37.15));
+    ASSERT_NEAR(1.56,  pidData[FD_PITCH].I, calculateTolerance(1.56));
+    EXPECT_FLOAT_EQ(0, pidData[FD_YAW].P);
+    EXPECT_FLOAT_EQ(0, pidData[FD_YAW].I);
+
+    // test FULL mode - expect P/I on all axes
+    unitLaunchControlMode = LAUNCH_CONTROL_MODE_FULL;
+    resetTest();
+    unitLaunchControlActive = true;
+    ENABLE_ARMING_FLAG(ARMED);
+    pidStabilisationState(PID_STABILISATION_ON);
+
+    pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
+
+    gyro.gyroADCf[FD_ROLL] = -20;
+    gyro.gyroADCf[FD_PITCH] = 20;
+    gyro.gyroADCf[FD_YAW] = -20;
+    pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
+
+    ASSERT_NEAR(25.62,  pidData[FD_ROLL].P,  calculateTolerance(25.62));
+    ASSERT_NEAR(1.56,   pidData[FD_ROLL].I,  calculateTolerance(1.56));
+    ASSERT_NEAR(-37.15, pidData[FD_PITCH].P, calculateTolerance(-37.15));
+    ASSERT_NEAR(-1.56,  pidData[FD_PITCH].I, calculateTolerance(-1.56));
+    ASSERT_NEAR(44.84,  pidData[FD_YAW].P,   calculateTolerance(44.84));
+    ASSERT_NEAR(1.56,   pidData[FD_YAW].I,  calculateTolerance(1.56));
+}