From b723d2976d1a7974f81072d88109d63bfbb6e6fd Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Tue, 30 Oct 2018 18:14:49 -0400 Subject: [PATCH] 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 | 20 +++--- src/main/fc/core.c | 14 ++-- src/main/fc/core.h | 7 -- src/main/flight/pid.c | 6 +- src/main/flight/pid.h | 4 +- src/main/interface/settings.c | 8 +-- src/main/interface/settings.h | 1 - src/test/Makefile | 3 +- src/test/unit/pid_unittest.cc | 124 +++++++++++++++++++++++++++++++++- 9 files changed, 146 insertions(+), 41 deletions(-) diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 848a96fd8c..b4adda071c 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/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_launchControlThrottlePct; +static uint8_t cmsx_launchControlAllowTriggerReset; +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_launchControlThrottlePct = pidProfile->launchControlThrottlePct; + cmsx_launchControlAllowTriggerReset = pidProfile->launchControlAllowTriggerReset; + 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->launchControlThrottlePct = cmsx_launchControlThrottlePct; + pidProfile->launchControlAllowTriggerReset = cmsx_launchControlAllowTriggerReset; + 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 }, - { "TRIGGER THROTTLE", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_launchControlThrottlePct, 0, 50, 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 }, + { "ALLOW RESET", OME_Bool, NULL, &cmsx_launchControlAllowTriggerReset, 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 }, + { "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 } }; diff --git a/src/main/fc/core.c b/src/main/fc/core.c index 9fcf3771bf..6da06df8e9 100644 --- a/src/main/fc/core.c +++ b/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 { diff --git a/src/main/fc/core.h b/src/main/fc/core.h index 94fa6a02e4..28a7e9a347 100644 --- a/src/main/fc/core.h +++ b/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); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 4afaf7fc06..ce08d753e1 100644 --- a/src/main/flight/pid.c +++ b/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; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 046a388646..329fbb773e 100644 --- a/src/main/flight/pid.h +++ b/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); diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 05fdda03f4..2f09bbf23f 100644 --- a/src/main/interface/settings.c +++ b/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_throttle_percent", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 50 }, PG_PID_PROFILE, offsetof(pidProfile_t, launchControlThrottlePct) }, + { "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, 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 diff --git a/src/main/interface/settings.h b/src/main/interface/settings.h index f0f8339a3a..e54d49efd9 100644 --- a/src/main/interface/settings.h +++ b/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; diff --git a/src/test/Makefile b/src/test/Makefile index 3f41de48a2..62ec370a3a 100644 --- a/src/test/Makefile +++ b/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 diff --git a/src/test/unit/pid_unittest.cc b/src/test/unit/pid_unittest.cc index 549a606197..33c0427294 100644 --- a/src/test/unit/pid_unittest.cc +++ b/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)); +}