From ff5c320b4a814e87893189bbc80566e58f142e84 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Mon, 23 May 2016 23:38:20 +0200 Subject: [PATCH] Iterm reset rework // Airmode Iterm Protection Configurable // Dynamic Ki New defaults --- src/main/blackbox/blackbox.c | 12 ++--- src/main/config/config.c | 17 ++++---- src/main/flight/mixer.c | 2 - src/main/flight/mixer.h | 1 - src/main/flight/pid.c | 85 +++++++++++++----------------------- src/main/flight/pid.h | 19 +++----- src/main/io/rc_controls.c | 9 ---- src/main/io/rc_controls.h | 1 - src/main/io/serial_cli.c | 9 ++-- src/main/mw.c | 30 +++++-------- src/main/rx/rx.h | 7 +-- src/main/version.h | 2 +- 12 files changed, 69 insertions(+), 125 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index ea43912a9..97ca2b619 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1280,24 +1280,24 @@ static bool blackboxWriteSysinfo() masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_average_count); break; case 36: - blackboxPrintfHeaderLine("dynamic_pterm:%d", - masterConfig.profile[masterConfig.current_profile_index].pidProfile.dynamic_pterm); + blackboxPrintfHeaderLine("dynamic_pid:%d", + masterConfig.profile[masterConfig.current_profile_index].pidProfile.dynamic_pid); break; case 37: blackboxPrintfHeaderLine("rollPitchItermResetRate:%d", - masterConfig.profile[masterConfig.current_profile_index].pidProfile.rollPitchItermResetRate); + masterConfig.profile[masterConfig.current_profile_index].pidProfile.rollPitchItermIgnoreRate); break; case 38: blackboxPrintfHeaderLine("yawItermResetRate:%d", - masterConfig.profile[masterConfig.current_profile_index].pidProfile.yawItermResetRate); + masterConfig.profile[masterConfig.current_profile_index].pidProfile.yawItermIgnoreRate); break; case 39: blackboxPrintfHeaderLine("dterm_lpf_hz:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_lpf_hz * 100.0f)); break; case 40: - blackboxPrintfHeaderLine("iterm_reset_offset:%d", - masterConfig.profile[masterConfig.current_profile_index].pidProfile.itermResetOffset); + blackboxPrintfHeaderLine("airmode_activate_throttle:%d", + masterConfig.rxConfig.airModeActivateThreshold); break; case 41: blackboxPrintfHeaderLine("deadband:%d", masterConfig.rcControlsConfig.deadband); diff --git a/src/main/config/config.c b/src/main/config/config.c index 691d87c15..63a239182 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -140,7 +140,7 @@ static uint32_t activeFeaturesLatch = 0; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 135; +static const uint8_t EEPROM_CONF_VERSION = 136; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { @@ -154,13 +154,13 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->pidController = 1; pidProfile->P8[ROLL] = 45; - pidProfile->I8[ROLL] = 35; + pidProfile->I8[ROLL] = 55; pidProfile->D8[ROLL] = 18; pidProfile->P8[PITCH] = 45; - pidProfile->I8[PITCH] = 35; + pidProfile->I8[PITCH] = 55; pidProfile->D8[PITCH] = 18; pidProfile->P8[YAW] = 90; - pidProfile->I8[YAW] = 40; + pidProfile->I8[YAW] = 50; pidProfile->D8[YAW] = 0; pidProfile->P8[PIDALT] = 50; pidProfile->I8[PIDALT] = 0; @@ -184,12 +184,10 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; pidProfile->yaw_lpf_hz = 80; - pidProfile->rollPitchItermResetRate = 200; - pidProfile->rollPitchItermResetAlways = 0; - pidProfile->yawItermResetRate = 50; - pidProfile->itermResetOffset = 15; + pidProfile->rollPitchItermIgnoreRate = 200; + pidProfile->yawItermIgnoreRate = 50;; pidProfile->dterm_lpf_hz = 110; // filtering ON by default - pidProfile->dynamic_pterm = 1; + pidProfile->dynamic_pid = 1; #ifdef GTUNE pidProfile->gtune_lolimP[ROLL] = 10; // [0..200] Lower limit of ROLL P during G tune. @@ -462,6 +460,7 @@ static void resetConf(void) masterConfig.rxConfig.fpvCamAngleDegrees = 0; masterConfig.rxConfig.max_aux_channel = 6; masterConfig.rxConfig.superExpoFactor = 30; + masterConfig.rxConfig.airModeActivateThreshold = 1350; masterConfig.rxConfig.superExpoFactorYaw = 30; masterConfig.rxConfig.superExpoYawMode = 0; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 17717247c..c777ca39f 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -822,7 +822,6 @@ void mixTable(void) throttleRange = throttleMax - throttleMin; if (rollPitchYawMixRange > throttleRange) { - motorLimitReached = true; mixReduction = qConstruct(throttleRange, rollPitchYawMixRange); for (i = 0; i < motorCount; i++) { @@ -833,7 +832,6 @@ void mixTable(void) if (debugMode == DEBUG_AIRMODE && i < 3) debug[1] = rollPitchYawMixRange; } else { - motorLimitReached = false; throttleMin = throttleMin + (rollPitchYawMixRange / 2); throttleMax = throttleMax - (rollPitchYawMixRange / 2); } diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 50cb3eb1c..a1f79d5a8 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -189,7 +189,6 @@ extern int16_t servo[MAX_SUPPORTED_SERVOS]; bool isMixerUsingServos(void); void writeServos(void); void filterServos(void); -bool motorLimitReached; #endif extern int16_t motor[MAX_SUPPORTED_MOTORS]; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 58e04898f..7e2a34623 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -49,7 +49,6 @@ #include "config/runtime_config.h" extern uint8_t motorCount; -extern bool motorLimitReached; uint32_t targetPidLooptime; int16_t axisPID[3]; @@ -61,13 +60,11 @@ int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; // PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction uint8_t PIDweight[3]; -static int32_t errorGyroI[3], errorGyroILimit[3]; +static int32_t errorGyroI[3]; #ifndef SKIP_PID_LUXFLOAT -static float errorGyroIf[3], errorGyroIfLimit[3]; +static float errorGyroIf[3]; #endif -static bool lowThrottlePidReduction; - static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig); @@ -101,22 +98,28 @@ uint16_t getDynamicKp(int axis, const pidProfile_t *pidProfile) { return dynamicKp; } -void pidResetErrorGyroState(uint8_t resetOption) -{ - if (resetOption >= RESET_ITERM) { - int axis; - for (axis = 0; axis < 3; axis++) { - errorGyroI[axis] = 0; -#ifndef SKIP_PID_LUXFLOAT - errorGyroIf[axis] = 0.0f; -#endif - } - } +uint16_t getDynamicKi(int axis, const pidProfile_t *pidProfile) { + uint16_t dynamicKi; + uint16_t resetRate; - if (resetOption == RESET_ITERM_AND_REDUCE_PID) { - lowThrottlePidReduction = true; - } else { - lowThrottlePidReduction = false; + resetRate = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; + + uint32_t dynamicFactor = (1 << 8) - constrain(ABS(gyroADC[axis] << 8) / resetRate, 0, 1 << 8); + + dynamicKi = (pidProfile->I8[axis] * dynamicFactor) >> 8; + + return dynamicKi; +} + +void pidResetErrorGyroState(void) +{ + int axis; + + for (axis = 0; axis < 3; axis++) { + errorGyroI[axis] = 0; +#ifndef SKIP_PID_LUXFLOAT + errorGyroIf[axis] = 0.0f; +#endif } } @@ -202,7 +205,7 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_ // multiplication of rcCommand corresponds to changing the sticks scaling here RateError = AngleRate - gyroRate; - uint16_t kP = (pidProfile->dynamic_pterm) ? getDynamicKp(axis, pidProfile) : pidProfile->P8[axis]; + uint16_t kP = (pidProfile->dynamic_pid) ? getDynamicKp(axis, pidProfile) : pidProfile->P8[axis]; // -----calculate P component if ((IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) || (axis == YAW && rxConfig->superExpoYawMode == SUPEREXPO_YAW_ALWAYS)) { @@ -217,21 +220,9 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_ } // -----calculate I component. - errorGyroIf[axis] = constrainf(errorGyroIf[axis] + luxITermScale * RateError * getdT() * pidProfile->I8[axis], -250.0f, 250.0f); + uint16_t kI = (pidProfile->dynamic_pid) ? getDynamicKi(axis, pidProfile) : pidProfile->I8[axis]; - if ((pidProfile->rollPitchItermResetAlways || IS_RC_MODE_ACTIVE(BOXSUPEREXPO)) && axis != YAW) { - if (ABS(gyroRate / 4.1f) >= pidProfile->rollPitchItermResetRate) errorGyroIf[axis] = constrainf(errorGyroIf[axis], -pidProfile->itermResetOffset, pidProfile->itermResetOffset); - } - - if (axis == YAW) { - if (ABS(gyroRate / 4.1f) >= pidProfile->yawItermResetRate) errorGyroIf[axis] = constrainf(errorGyroIf[axis], -YAW_ITERM_RESET_OFFSET, YAW_ITERM_RESET_OFFSET); - } - - if (antiWindupProtection || motorLimitReached) { - errorGyroIf[axis] = constrainf(errorGyroIf[axis], -errorGyroIfLimit[axis], errorGyroIfLimit[axis]); - } else { - errorGyroIfLimit[axis] = ABS(errorGyroIf[axis]); - } + errorGyroIf[axis] = constrainf(kI + luxITermScale * RateError * getdT() * pidProfile->I8[axis], -250.0f, 250.0f); // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. // I coefficient (I8) moved before integration to make limiting independent from PID settings @@ -257,8 +248,6 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_ // -----calculate total PID output axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000); - if (lowThrottlePidReduction) axisPID[axis] /= 3; - #ifdef GTUNE if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { calculate_Gtune(axis); @@ -333,7 +322,7 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRate gyroRate = gyroADC[axis] / 4; RateError = AngleRateTmp - gyroRate; - uint16_t kP = (pidProfile->dynamic_pterm) ? getDynamicKp(axis, pidProfile) : pidProfile->P8[axis]; + uint16_t kP = (pidProfile->dynamic_pid) ? getDynamicKp(axis, pidProfile) : pidProfile->P8[axis]; // -----calculate P component if ((IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) || (axis == YAW && rxConfig->superExpoYawMode == SUPEREXPO_YAW_ALWAYS)) { @@ -352,26 +341,14 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRate // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used. // Time correction (to avoid different I scaling for different builds based on average cycle time) // is normalized to cycle time = 2048. - errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)targetPidLooptime) >> 11) * pidProfile->I8[axis]; + uint16_t kI = (pidProfile->dynamic_pid) ? getDynamicKi(axis, pidProfile) : pidProfile->I8[axis]; + + errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)targetPidLooptime) >> 11) * kI; // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. // I coefficient (I8) moved before integration to make limiting independent from PID settings errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13); - if ((pidProfile->rollPitchItermResetAlways || IS_RC_MODE_ACTIVE(BOXSUPEREXPO)) && axis != YAW) { - if (ABS(gyroRate *10 / 41) >= pidProfile->rollPitchItermResetRate) errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) -pidProfile->itermResetOffset << 13, (int32_t) + pidProfile->itermResetOffset << 13); - } - - if (axis == YAW) { - if (ABS(gyroRate * 10 / 41) >= pidProfile->yawItermResetRate) errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) -YAW_ITERM_RESET_OFFSET << 13, (int32_t) + YAW_ITERM_RESET_OFFSET << 13); - } - - if (antiWindupProtection || motorLimitReached) { - errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]); - } else { - errorGyroILimit[axis] = ABS(errorGyroI[axis]); - } - ITerm = errorGyroI[axis] >> 13; //-----calculate D-term @@ -394,8 +371,6 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRate // -----calculate total PID output axisPID[axis] = PTerm + ITerm + DTerm; - if (lowThrottlePidReduction) axisPID[axis] /= 3; - #ifdef GTUNE if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { calculate_Gtune(axis); diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 046adbcb6..30bffb98f 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -22,7 +22,6 @@ #define YAW_P_LIMIT_MIN 100 // Maximum value for yaw P limiter #define YAW_P_LIMIT_MAX 500 // Maximum value for yaw P limiter -#define YAW_ITERM_RESET_OFFSET 15 // May be made configurable in the future, but not really needed for yaw #define DYNAMIC_PTERM_STICK_THRESHOLD 400 typedef enum { @@ -50,12 +49,6 @@ typedef enum { DELTA_FROM_MEASUREMENT } pidDeltaType_e; -typedef enum { - RESET_DISABLE = 0, - RESET_ITERM, - RESET_ITERM_AND_REDUCE_PID -} pidErrorResetOption_e; - typedef enum { SUPEREXPO_YAW_OFF = 0, SUPEREXPO_YAW_ON, @@ -71,15 +64,13 @@ typedef struct pidProfile_s { uint8_t I8[PID_ITEM_COUNT]; uint8_t D8[PID_ITEM_COUNT]; - uint8_t itermResetOffset; // Reset offset for Iterm uint16_t dterm_lpf_hz; // Delta Filter in hz uint16_t yaw_lpf_hz; // Additional yaw filter when yaw axis too noisy - uint16_t rollPitchItermResetRate; // Experimental threshold for resetting iterm for pitch and roll on certain rates - uint8_t rollPitchItermResetAlways; // Reset Iterm also without SUPER EXPO - uint16_t yawItermResetRate; // Experimental threshold for resetting iterm for yaw on certain rates + uint16_t rollPitchItermIgnoreRate; // Experimental threshold for resetting iterm for pitch and roll on certain rates + uint16_t yawItermIgnoreRate; // Experimental threshold for resetting iterm for yaw on certain rates uint16_t yaw_p_limit; uint8_t dterm_average_count; // Configurable delta count for dterm - uint8_t dynamic_pterm; + uint8_t dynamic_pid; // Dynamic PID implementation (currently only P and I) #ifdef GTUNE uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune @@ -98,10 +89,10 @@ typedef void (*pidControllerFuncPtr)(const pidProfile_t *pidProfile, const struc extern int16_t axisPID[XYZ_AXIS_COUNT]; extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; -bool antiWindupProtection; +bool airmodeWasActivated; extern uint32_t targetPidLooptime; void pidSetController(pidControllerType_e type); -void pidResetErrorGyroState(uint8_t resetOption); +void pidResetErrorGyroState(void); void setTargetPidLooptime(uint8_t pidProcessDenom); diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 3264b0269..fdde2cf1a 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -124,15 +124,6 @@ throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband return THROTTLE_HIGH; } -rollPitchStatus_e calculateRollPitchCenterStatus(rxConfig_t *rxConfig) -{ - if (((rcData[PITCH] < (rxConfig->midrc + AIRMODEDEADBAND)) && (rcData[PITCH] > (rxConfig->midrc -AIRMODEDEADBAND))) - && ((rcData[ROLL] < (rxConfig->midrc + AIRMODEDEADBAND)) && (rcData[ROLL] > (rxConfig->midrc -AIRMODEDEADBAND)))) - return CENTERED; - - return NOT_CENTERED; -} - void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch) { static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index 7756aed07..eaec277ae 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -252,4 +252,3 @@ bool isUsingSticksForArming(void); int32_t getRcStickDeflection(int32_t axis, uint16_t midrc); bool isModeActivationConditionPresent(modeActivationCondition_t *modeActivationConditions, boxId_e modeId); -rollPitchStatus_e calculateRollPitchCenterStatus(rxConfig_t *rxConfig); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 1ea06c9f0..6ecc665b1 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -698,6 +698,7 @@ const clivalue_t valueTable[] = { { "super_expo_factor", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.superExpoFactor, .config.minmax = {1, 100 } }, { "super_expo_factor_yaw", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.superExpoFactorYaw, .config.minmax = {1, 100 } }, { "super_expo_yaw", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.superExpoYawMode, .config.lookup = { TABLE_SUPEREXPO_YAW } }, + { "airmode_activate_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.airModeActivateThreshold, .config.minmax = {1000, 2000 } }, { "failsafe_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_delay, .config.minmax = { 0, 200 } }, { "failsafe_off_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_off_delay, .config.minmax = { 0, 200 } }, @@ -730,11 +731,9 @@ const clivalue_t valueTable[] = { { "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } }, { "mag_declination", VAR_INT16 | MASTER_VALUE, &masterConfig.mag_declination, .config.minmax = { -18000, 18000 } }, { "dterm_lowpass", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, - { "dynamic_pterm", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dynamic_pterm, .config.lookup = { TABLE_OFF_ON } }, - { "iterm_always_reset", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.rollPitchItermResetAlways, .config.lookup = { TABLE_OFF_ON } }, - { "iterm_reset_degrees", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermResetRate, .config.minmax = {50, 1000 } }, - { "yaw_iterm_reset_degrees", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermResetRate, .config.minmax = {25, 1000 } }, - { "iterm_reset_offset", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermResetOffset, .config.minmax = { 0, 100 } }, + { "dynamic_pid", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dynamic_pid, .config.lookup = { TABLE_OFF_ON } }, + { "iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {50, 1000 } }, + { "yaw_iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermIgnoreRate, .config.minmax = {25, 1000 } }, { "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } }, { "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.pid_process_denom, .config.minmax = { 1, 8 } }, diff --git a/src/main/mw.c b/src/main/mw.c index b9da3e6b7..125674c9e 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -101,6 +101,8 @@ enum { #define GYRO_WATCHDOG_DELAY 80 // delay for gyro sync +#define AIRMODE_THOTTLE_THRESHOLD 1350 // Make configurable in the future. ~35% throttle should be fine + uint16_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop int16_t magHold; @@ -113,7 +115,6 @@ static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the m extern uint32_t currentTime; extern uint8_t PIDweight[3]; -extern bool antiWindupProtection; uint16_t filteredCycleTime; static bool isRXDataNew; @@ -453,6 +454,7 @@ void updateMagHold(void) void processRx(void) { static bool armedBeeperOn = false; + static bool airmodeIsActivated; calculateRxChannelsAndUpdateFailsafe(currentTime); @@ -474,27 +476,17 @@ void processRx(void) } throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); - rollPitchStatus_e rollPitchStatus = calculateRollPitchCenterStatus(&masterConfig.rxConfig); + + if (IS_RC_MODE_ACTIVE(BOXAIRMODE) && ARMING_FLAG(ARMED)) { + if (rcCommand[THROTTLE] >= masterConfig.rxConfig.airModeActivateThreshold) airmodeIsActivated = true; // Prevent Iterm from being reset + } else { + airmodeIsActivated = false; + } /* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered. This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air */ - if (throttleStatus == THROTTLE_LOW) { - if (IS_RC_MODE_ACTIVE(BOXAIRMODE) && !failsafeIsActive() && ARMING_FLAG(ARMED)) { - if (rollPitchStatus == CENTERED) { - antiWindupProtection = true; - } else { - antiWindupProtection = false; - } - } else { - if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) { - pidResetErrorGyroState(RESET_ITERM); - } else { - pidResetErrorGyroState(RESET_ITERM_AND_REDUCE_PID); - } - } - } else { - pidResetErrorGyroState(RESET_DISABLE); - antiWindupProtection = false; + if (throttleStatus == THROTTLE_LOW && !airmodeIsActivated) { + pidResetErrorGyroState(); } // When armed and motors aren't spinning, do beeps and then disarm diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 6e2dd2124..b6e1cfea3 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -124,9 +124,10 @@ typedef struct rxConfig_s { uint8_t rcSmoothing; uint8_t fpvCamAngleDegrees; // Camera angle to be scaled into rc commands uint8_t max_aux_channel; - uint8_t superExpoFactor; // Super Expo Factor - uint8_t superExpoFactorYaw; // Super Expo Factor Yaw - uint8_t superExpoYawMode; // Seperate Super expo for yaw + uint8_t superExpoFactor; // Super Expo Factor + uint8_t superExpoFactorYaw; // Super Expo Factor Yaw + uint8_t superExpoYawMode; // Seperate Super expo for yaw + uint16_t airModeActivateThreshold; // Throttle setpoint where airmode gets activated uint16_t rx_min_usec; uint16_t rx_max_usec; diff --git a/src/main/version.h b/src/main/version.h index 009ff6e35..fd95e254d 100644 --- a/src/main/version.h +++ b/src/main/version.h @@ -17,7 +17,7 @@ #define FC_VERSION_MAJOR 2 // increment when a major release is made (big new feature, etc) #define FC_VERSION_MINOR 7 // increment when a minor release is made (small new feature, change etc) -#define FC_VERSION_PATCH_LEVEL 1 // increment when a bug is fixed +#define FC_VERSION_PATCH_LEVEL 2 // increment when a bug is fixed #define STR_HELPER(x) #x #define STR(x) STR_HELPER(x)