Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 20:35:33 +03:00
Code Issues Wiki Activity

Changes from review and add unit tests

Browse source 
Stylistic updates and add tests to the PID unit tests to verify launch control behavior
This commit is contained in:
Bruce Luckcuck 2018-10-30 18:14:49 -04:00
parent e4dc93b128
commit b723d2976d
9 changed files with 146 additions and 41 deletions
Download patch file Download diff file Expand all files Collapse all files

20
src/main/cms/cms_menu_imu.c
View file

@ -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 }
};

14
src/main/fc/core.c
View file

@ -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 {

7
src/main/fc/core.h
View file

@ -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);

6
src/main/flight/pid.c
View file

@ -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;

4
src/main/flight/pid.h
View file

@ -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);

8
src/main/interface/settings.c
View file

@ -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

1
src/main/interface/settings.h
View file

@ -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;

3
src/test/Makefile
View file

@ -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

124
src/test/unit/pid_unittest.cc
View file

@ -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));
}