diff --git a/src/main/config/config.c b/src/main/config/config.c index d74f714af..0d8c47eeb 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -110,7 +110,7 @@ profile_t *currentProfile; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 90; +static const uint8_t EEPROM_CONF_VERSION = 91; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { @@ -121,6 +121,8 @@ static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) static void resetPidProfile(pidProfile_t *pidProfile) { + pidProfile->pidController = 0; + pidProfile->P8[ROLL] = 40; pidProfile->I8[ROLL] = 30; pidProfile->D8[ROLL] = 23; @@ -386,7 +388,6 @@ static void resetConf(void) masterConfig.looptime = 3500; masterConfig.emf_avoidance = 0; - currentProfile->pidController = 0; resetPidProfile(¤tProfile->pidProfile); resetControlRateConfig(&masterConfig.controlRateProfiles[0]); @@ -603,7 +604,7 @@ void activateConfig(void) useTelemetryConfig(&masterConfig.telemetryConfig); #endif - setPIDController(currentProfile->pidController); + setPIDController(currentProfile->pidProfile.pidController); #ifdef GPS gpsUseProfile(¤tProfile->gpsProfile); diff --git a/src/main/config/config_profile.h b/src/main/config/config_profile.h index 1aafc14bd..7968b97c0 100644 --- a/src/main/config/config_profile.h +++ b/src/main/config/config_profile.h @@ -18,8 +18,6 @@ #pragma once typedef struct profile_s { - uint8_t pidController; // 0 = multiwii original, 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671, 1, 2 = Luggi09s new baseflight pid - pidProfile_t pidProfile; uint8_t defaultRateProfileIndex; diff --git a/src/main/flight/autotune.c b/src/main/flight/autotune.c index 2770b6824..4926848d8 100644 --- a/src/main/flight/autotune.c +++ b/src/main/flight/autotune.c @@ -415,7 +415,7 @@ void restorePids(pidProfile_t *pidProfileToTune) memcpy(pidProfileToTune, &pidBackup, sizeof(pidBackup)); } -void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune, uint8_t pidControllerInUse) +void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune) { phase = nextPhase; @@ -439,7 +439,7 @@ void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune, uint8_t pidControlle firstPeakAngle = secondPeakAngle = 0; pidProfile = pidProfileToTune; - pidController = pidControllerInUse; + pidController = pidProfile->pidController; updatePidIndex(); updateTargetAngle(); diff --git a/src/main/flight/autotune.h b/src/main/flight/autotune.h index 095309aaf..8d4296e3c 100644 --- a/src/main/flight/autotune.h +++ b/src/main/flight/autotune.h @@ -18,7 +18,7 @@ #pragma once void autotuneReset(); -void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune, uint8_t pidControllerInUse); +void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune); float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclination, float errorAngle); void autotuneEndPhase(); diff --git a/src/main/flight/flight.h b/src/main/flight/flight.h index 51fd74a0a..cad033b4f 100644 --- a/src/main/flight/flight.h +++ b/src/main/flight/flight.h @@ -32,12 +32,16 @@ typedef enum { PID_ITEM_COUNT } pidIndex_e; +#define IS_PID_CONTROLLER_FP_BASED(pidController) (pidController == 2) + typedef struct pidProfile_s { + uint8_t pidController; // 0 = multiwii original, 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671, 1, 2 = Luggi09s new baseflight pid + uint8_t P8[PID_ITEM_COUNT]; uint8_t I8[PID_ITEM_COUNT]; uint8_t D8[PID_ITEM_COUNT]; - float P_f[3]; // float p i and d factors for the new baseflight pid + float P_f[3]; // float p i and d factors for lux float pid controller float I_f[3]; float D_f[3]; float A_level; diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 64a70ba40..5a821a5cd 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -21,6 +21,8 @@ #include +#include "platform.h" + #include "common/axis.h" #include "common/maths.h" @@ -360,6 +362,7 @@ void configureAdjustment(uint8_t index, uint8_t auxSwitchChannelIndex, const adj void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta) { int newValue; + float newFloatValue; if (delta > 0) { queueConfirmationBeep(2); @@ -391,34 +394,70 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm controlRateConfig->yawRate = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c break; case ADJUSTMENT_PITCH_ROLL_P: - newValue = (int)pidProfile->P8[PIDPITCH] + delta; - pidProfile->P8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c - newValue = (int)pidProfile->P8[PIDROLL] + delta; - pidProfile->P8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { + newFloatValue = (int)pidProfile->P_f[PIDPITCH] + delta; + pidProfile->P_f[PIDPITCH] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = (int)pidProfile->P_f[PIDROLL] + delta; + pidProfile->P_f[PIDROLL] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + } else { + newValue = (int)pidProfile->P8[PIDPITCH] + delta; + pidProfile->P8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + newValue = (int)pidProfile->P8[PIDROLL] + delta; + pidProfile->P8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + } break; case ADJUSTMENT_PITCH_ROLL_I: - newValue = (int)pidProfile->I8[PIDPITCH] + delta; - pidProfile->I8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c - newValue = (int)pidProfile->I8[PIDROLL] + delta; - pidProfile->I8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { + newFloatValue = (int)pidProfile->I_f[PIDPITCH] + delta; + pidProfile->I_f[PIDPITCH] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = (int)pidProfile->I_f[PIDROLL] + delta; + pidProfile->I_f[PIDROLL] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + } else { + newValue = (int)pidProfile->I8[PIDPITCH] + delta; + pidProfile->I8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + newValue = (int)pidProfile->I8[PIDROLL] + delta; + pidProfile->I8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + } break; case ADJUSTMENT_PITCH_ROLL_D: - newValue = (int)pidProfile->D8[PIDPITCH] + delta; - pidProfile->D8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c - newValue = (int)pidProfile->D8[PIDROLL] + delta; - pidProfile->D8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { + newFloatValue = (int)pidProfile->D_f[PIDPITCH] + delta; + pidProfile->D_f[PIDPITCH] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = (int)pidProfile->D_f[PIDROLL] + delta; + pidProfile->D_f[PIDROLL] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + } else { + newValue = (int)pidProfile->D8[PIDPITCH] + delta; + pidProfile->D8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + newValue = (int)pidProfile->D8[PIDROLL] + delta; + pidProfile->D8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + } break; case ADJUSTMENT_YAW_P: - newValue = (int)pidProfile->P8[PIDYAW] + delta; - pidProfile->P8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { + newFloatValue = (int)pidProfile->P_f[PIDYAW] + delta; + pidProfile->P_f[PIDYAW] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + } else { + newValue = (int)pidProfile->P8[PIDYAW] + delta; + pidProfile->P8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + } break; case ADJUSTMENT_YAW_I: - newValue = (int)pidProfile->I8[PIDYAW] + delta; - pidProfile->I8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { + newFloatValue = (int)pidProfile->I_f[PIDYAW] + delta; + pidProfile->I_f[PIDYAW] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + } else { + newValue = (int)pidProfile->I8[PIDYAW] + delta; + pidProfile->I8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + } break; case ADJUSTMENT_YAW_D: - newValue = (int)pidProfile->D8[PIDYAW] + delta; - pidProfile->D8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { + newFloatValue = (int)pidProfile->D_f[PIDYAW] + delta; + pidProfile->D_f[PIDYAW] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + } else { + newValue = (int)pidProfile->D8[PIDYAW] + delta; + pidProfile->D8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + } break; default: break; diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index a1c2a10a5..dde0536f0 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -207,7 +207,10 @@ typedef struct adjustmentState_s { uint32_t timeoutAt; } adjustmentState_t; + +#ifndef MAX_SIMULTANEOUS_ADJUSTMENT_COUNT #define MAX_SIMULTANEOUS_ADJUSTMENT_COUNT 4 // enough for 4 x 3position switches / 4 aux channel +#endif #define MAX_ADJUSTMENT_RANGE_COUNT 12 // enough for 2 * 6pos switches. diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 2ec7e39f4..a8eaf45e6 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -370,7 +370,7 @@ const clivalue_t valueTable[] = { { "mag_hardware", VAR_UINT8 | MASTER_VALUE, &masterConfig.mag_hardware, 0, MAG_NONE }, { "mag_declination", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].mag_declination, -18000, 18000 }, - { "pid_controller", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidController, 0, 5 }, + { "pid_controller", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidController, 0, 5 }, { "p_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PITCH], 0, 200 }, { "i_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PITCH], 0, 200 }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 5622da838..97822698a 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -865,7 +865,7 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_PID: headSerialReply(3 * PID_ITEM_COUNT); - if (currentProfile->pidController == 2) { // convert float stuff into uint8_t to keep backwards compatability with all 8-bit shit with new pid + if (currentProfile->pidProfile.pidController == 2) { // convert float stuff into uint8_t to keep backwards compatability with all 8-bit shit with new pid for (i = 0; i < 3; i++) { serialize8(constrain(lrintf(currentProfile->pidProfile.P_f[i] * 10.0f), 0, 250)); serialize8(constrain(lrintf(currentProfile->pidProfile.I_f[i] * 100.0f), 0, 250)); @@ -896,7 +896,7 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_PID_CONTROLLER: headSerialReply(1); - serialize8(currentProfile->pidController); + serialize8(currentProfile->pidProfile.pidController); break; case MSP_MODE_RANGES: headSerialReply(4 * MAX_MODE_ACTIVATION_CONDITION_COUNT); @@ -1205,11 +1205,11 @@ static bool processInCommand(void) currentProfile->accelerometerTrims.values.roll = read16(); break; case MSP_SET_PID_CONTROLLER: - currentProfile->pidController = read8(); - setPIDController(currentProfile->pidController); + currentProfile->pidProfile.pidController = read8(); + setPIDController(currentProfile->pidProfile.pidController); break; case MSP_SET_PID: - if (currentProfile->pidController == 2) { + if (currentProfile->pidProfile.pidController == 2) { for (i = 0; i < 3; i++) { currentProfile->pidProfile.P_f[i] = (float)read8() / 10.0f; currentProfile->pidProfile.I_f[i] = (float)read8() / 100.0f; diff --git a/src/main/mw.c b/src/main/mw.c index 0d542311c..028177515 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -124,7 +124,7 @@ void updateAutotuneState(void) autotuneReset(); landedAfterAutoTuning = false; } - autotuneBeginNextPhase(¤tProfile->pidProfile, currentProfile->pidController); + autotuneBeginNextPhase(¤tProfile->pidProfile); ENABLE_FLIGHT_MODE(AUTOTUNE_MODE); autoTuneWasUsed = true; } else { diff --git a/src/test/unit/platform.h b/src/test/unit/platform.h index 39c809316..f74b9be29 100644 --- a/src/test/unit/platform.h +++ b/src/test/unit/platform.h @@ -25,3 +25,4 @@ #define SERIAL_PORT_COUNT 4 +#define MAX_SIMULTANEOUS_ADJUSTMENT_COUNT 6 diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index 54a830278..91ec4f515 100644 --- a/src/test/unit/rc_controls_unittest.cc +++ b/src/test/unit/rc_controls_unittest.cc @@ -17,7 +17,9 @@ #include #include + extern "C" { + #include "platform.h" #include "common/axis.h" #include "flight/flight.h" @@ -29,6 +31,8 @@ extern "C" { #include "gtest/gtest.h" extern "C" { +extern void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfig, pidProfile_t *pidProfile); + int constrain(int amt, int low, int high) { if (amt < low) @@ -479,6 +483,215 @@ TEST(RcControlsTest, processRcRateProfileAdjustments) EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); } +static const adjustmentConfig_t pidPitchAndRollPAdjustmentConfig = { + .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_P, + .mode = ADJUSTMENT_MODE_STEP, + .data = { { 1 } } +}; + +static const adjustmentConfig_t pidPitchAndRollIAdjustmentConfig = { + .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_I, + .mode = ADJUSTMENT_MODE_STEP, + .data = { { 1 } } +}; + +static const adjustmentConfig_t pidPitchAndRollDAdjustmentConfig = { + .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_D, + .mode = ADJUSTMENT_MODE_STEP, + .data = { { 1 } } +}; + +static const adjustmentConfig_t pidYawPAdjustmentConfig = { + .adjustmentFunction = ADJUSTMENT_YAW_P, + .mode = ADJUSTMENT_MODE_STEP, + .data = { { 1 } } +}; + +static const adjustmentConfig_t pidYawIAdjustmentConfig = { + .adjustmentFunction = ADJUSTMENT_YAW_I, + .mode = ADJUSTMENT_MODE_STEP, + .data = { { 1 } } +}; + +static const adjustmentConfig_t pidYawDAdjustmentConfig = { + .adjustmentFunction = ADJUSTMENT_YAW_D, + .mode = ADJUSTMENT_MODE_STEP, + .data = { { 1 } } +}; + +TEST(RcControlsTest, processPIDIncreasePidController0) +{ + // given + modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT]; + memset(&modeActivationConditions, 0, sizeof (modeActivationConditions)); + + escAndServoConfig_t escAndServoConfig; + memset(&escAndServoConfig, 0, sizeof (escAndServoConfig)); + + pidProfile_t pidProfile; + memset(&pidProfile, 0, sizeof (pidProfile)); + pidProfile.pidController = 0; + pidProfile.P8[PIDPITCH] = 0; + pidProfile.P8[PIDROLL] = 5; + pidProfile.P8[YAW] = 7; + pidProfile.I8[PIDPITCH] = 10; + pidProfile.I8[PIDROLL] = 15; + pidProfile.I8[YAW] = 17; + pidProfile.D8[PIDPITCH] = 20; + pidProfile.D8[PIDROLL] = 25; + pidProfile.D8[YAW] = 27; + + // and + controlRateConfig_t controlRateConfig; + memset(&controlRateConfig, 0, sizeof (controlRateConfig)); + + // and + memset(&rxConfig, 0, sizeof (rxConfig)); + rxConfig.mincheck = DEFAULT_MIN_CHECK; + rxConfig.maxcheck = DEFAULT_MAX_CHECK; + rxConfig.midrc = 1500; + + adjustmentStateMask = 0; + memset(&adjustmentStates, 0, sizeof(adjustmentStates)); + + configureAdjustment(0, AUX1 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollPAdjustmentConfig); + configureAdjustment(1, AUX2 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollIAdjustmentConfig); + configureAdjustment(2, AUX3 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollDAdjustmentConfig); + configureAdjustment(3, AUX1 - NON_AUX_CHANNEL_COUNT, &pidYawPAdjustmentConfig); + configureAdjustment(4, AUX2 - NON_AUX_CHANNEL_COUNT, &pidYawIAdjustmentConfig); + configureAdjustment(5, AUX3 - NON_AUX_CHANNEL_COUNT, &pidYawDAdjustmentConfig); + + // and + uint8_t index; + for (index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) { + rcData[index] = PWM_RANGE_MIDDLE; + } + + // and + resetCallCounters(); + resetMillis(); + + // and + rcData[AUX1] = PWM_RANGE_MAX; + rcData[AUX2] = PWM_RANGE_MAX; + rcData[AUX3] = PWM_RANGE_MAX; + + // and + uint8_t expectedAdjustmentStateMask = + (1 << 0) | + (1 << 1) | + (1 << 2) | + (1 << 3) | + (1 << 4) | + (1 << 5); + + // when + useRcControlsConfig(modeActivationConditions, &escAndServoConfig, &pidProfile); + processRcAdjustments(&controlRateConfig, &rxConfig); + + // then + EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 6); + EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); + + // and + EXPECT_EQ(pidProfile.P8[PIDPITCH], 1); + EXPECT_EQ(pidProfile.P8[PIDROLL], 6); + EXPECT_EQ(pidProfile.P8[YAW], 8); + EXPECT_EQ(pidProfile.I8[PIDPITCH], 11); + EXPECT_EQ(pidProfile.I8[PIDROLL], 16); + EXPECT_EQ(pidProfile.I8[YAW], 18); + EXPECT_EQ(pidProfile.D8[PIDPITCH], 21); + EXPECT_EQ(pidProfile.D8[PIDROLL], 26); + EXPECT_EQ(pidProfile.D8[YAW], 28); +} + +TEST(RcControlsTest, processPIDIncreasePidController2) +{ + // given + modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT]; + memset(&modeActivationConditions, 0, sizeof (modeActivationConditions)); + + escAndServoConfig_t escAndServoConfig; + memset(&escAndServoConfig, 0, sizeof (escAndServoConfig)); + + pidProfile_t pidProfile; + memset(&pidProfile, 0, sizeof (pidProfile)); + pidProfile.pidController = 2; + pidProfile.P_f[PIDPITCH] = 0.0f; + pidProfile.P_f[PIDROLL] = 5.0f; + pidProfile.P_f[PIDYAW] = 7.0f; + pidProfile.I_f[PIDPITCH] = 10.0f; + pidProfile.I_f[PIDROLL] = 15.0f; + pidProfile.I_f[PIDYAW] = 17.0f; + pidProfile.D_f[PIDPITCH] = 20.0f; + pidProfile.D_f[PIDROLL] = 25.0f; + pidProfile.D_f[PIDYAW] = 27.0f; + + // and + controlRateConfig_t controlRateConfig; + memset(&controlRateConfig, 0, sizeof (controlRateConfig)); + + // and + memset(&rxConfig, 0, sizeof (rxConfig)); + rxConfig.mincheck = DEFAULT_MIN_CHECK; + rxConfig.maxcheck = DEFAULT_MAX_CHECK; + rxConfig.midrc = 1500; + + adjustmentStateMask = 0; + memset(&adjustmentStates, 0, sizeof(adjustmentStates)); + + configureAdjustment(0, AUX1 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollPAdjustmentConfig); + configureAdjustment(1, AUX2 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollIAdjustmentConfig); + configureAdjustment(2, AUX3 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollDAdjustmentConfig); + configureAdjustment(3, AUX1 - NON_AUX_CHANNEL_COUNT, &pidYawPAdjustmentConfig); + configureAdjustment(4, AUX2 - NON_AUX_CHANNEL_COUNT, &pidYawIAdjustmentConfig); + configureAdjustment(5, AUX3 - NON_AUX_CHANNEL_COUNT, &pidYawDAdjustmentConfig); + + // and + uint8_t index; + for (index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) { + rcData[index] = PWM_RANGE_MIDDLE; + } + + // and + resetCallCounters(); + resetMillis(); + + // and + rcData[AUX1] = PWM_RANGE_MAX; + rcData[AUX2] = PWM_RANGE_MAX; + rcData[AUX3] = PWM_RANGE_MAX; + + // and + uint8_t expectedAdjustmentStateMask = + (1 << 0) | + (1 << 1) | + (1 << 2) | + (1 << 3) | + (1 << 4) | + (1 << 5); + + // when + useRcControlsConfig(modeActivationConditions, &escAndServoConfig, &pidProfile); + processRcAdjustments(&controlRateConfig, &rxConfig); + + // then + EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 6); + EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); + + // and + EXPECT_EQ(pidProfile.P_f[PIDPITCH], 1.0f); + EXPECT_EQ(pidProfile.P_f[PIDROLL], 6.0f); + EXPECT_EQ(pidProfile.P_f[PIDYAW], 8.0f); + EXPECT_EQ(pidProfile.I_f[PIDPITCH], 11.0f); + EXPECT_EQ(pidProfile.I_f[PIDROLL], 16.0f); + EXPECT_EQ(pidProfile.I_f[PIDYAW], 18.0f); + EXPECT_EQ(pidProfile.D_f[PIDPITCH], 21.0f); + EXPECT_EQ(pidProfile.D_f[PIDROLL], 26.0f); + EXPECT_EQ(pidProfile.D_f[PIDYAW], 28.0f); + +} + extern "C" { void saveConfigAndNotify(void) {} void generateThrottleCurve(controlRateConfig_t *, escAndServoConfig_t *) {} @@ -494,6 +707,8 @@ void mwDisarm(void) {} uint8_t getCurrentControlRateProfile(void) { return 0; } +void GPS_reset_home_position(void) {} +void baroSetCalibrationCycles(uint16_t) {} uint8_t armingFlags = 0; int16_t heading;