diff --git a/src/main/config/config.c b/src/main/config/config.c index be9d615ed3..1b7c714790 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -220,6 +220,7 @@ void resetPidProfile(pidProfile_t *pidProfile) pidProfile->dterm_lpf_hz = 100; // filtering ON by default pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT; pidProfile->vbatPidCompensation = 0; + pidProfile->zeroThrottleStabilisation = PID_STABILISATION_OFF; // Betaflight PID controller parameters pidProfile->toleranceBand = 15; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index aa9f0fc90a..f8ac94b22b 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -70,8 +70,6 @@ static bool syncPwmWithPidLoop = false; static mixerMode_e currentMixerMode; static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; -float errorLimiter = 1.0f; - #ifdef USE_SERVOS static uint8_t servoRuleCount = 0; static servoMixer_t currentServoMixer[MAX_SERVO_RULES]; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 3907e3955d..d23db67a3d 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -49,9 +49,10 @@ extern uint8_t motorCount; uint32_t targetPidLooptime; -extern float errorLimiter; extern float angleRate[3], angleRateSmooth[3]; +static bool pidStabilisationEnabled; + int16_t axisPID[3]; #ifdef BLACKBOX @@ -76,14 +77,17 @@ void setTargetPidLooptime(uint8_t pidProcessDenom) void pidResetErrorGyroState(void) { - int axis; - - for (axis = 0; axis < 3; axis++) { + for (int axis = 0; axis < 3; axis++) { errorGyroI[axis] = 0; errorGyroIf[axis] = 0.0f; } } +void pidStabilisationState(pidStabilisationState_e pidControllerState) +{ + pidStabilisationEnabled = (pidControllerState == PID_STABILISATION_ON) ? true : false; +} + float getdT (void) { static float dT; @@ -269,6 +273,8 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000); } + if (!pidStabilisationEnabled) axisPID[axis] = 0; + #ifdef GTUNE if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { calculate_Gtune(axis); @@ -409,6 +415,7 @@ static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclina axisPID[axis] = PTerm + ITerm + DTerm; } + if (!pidStabilisationEnabled) axisPID[axis] = 0; #ifdef GTUNE if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 56767f8719..64182f8256 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -62,6 +62,11 @@ typedef enum { POSITIVE_ERROR } pidErrorPolarity_e; +typedef enum { + PID_STABILISATION_OFF = 0, + PID_STABILISATION_ON +} pidStabilisationState_e; + typedef struct pidProfile_s { uint8_t pidController; // 1 = rewrite betaflight evolved from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671, 2 = Betaflight PIDc (Evolved Luxfloat) @@ -77,6 +82,7 @@ typedef struct pidProfile_s { uint16_t yaw_p_limit; uint8_t dterm_average_count; // Configurable delta count for dterm uint8_t vbatPidCompensation; // Scale PIDsum to battery voltage + uint8_t zeroThrottleStabilisation; // Disable/Enable zero throttle stabilisation. Normally even without airmode P and D would be active. // Betaflight PID controller parameters uint8_t toleranceBand; // Error tolerance area where toleranceBandReduction is applied under certain circumstances @@ -107,5 +113,6 @@ extern uint32_t targetPidLooptime; void pidSetController(pidControllerType_e type); void pidResetErrorGyroState(void); +void pidStabilisationState(pidStabilisationState_e pidControllerState); void setTargetPidLooptime(uint8_t pidProcessDenom); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index ec06f6fcc2..768489a8de 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -766,6 +766,7 @@ const clivalue_t valueTable[] = { { "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 } }, { "vbat_pid_compensation", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.vbatPidCompensation, .config.lookup = { TABLE_OFF_ON } }, + { "zero_throttle_stabilisation",VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.zeroThrottleStabilisation, .config.lookup = { TABLE_OFF_ON } }, { "motor_accel_limit_percent", VAR_UINT16 | MASTER_VALUE, &masterConfig.profile[0].pidProfile.accelerationLimitPercent, .config.minmax = { 0, 10000 } }, { "pid_tolerance_band", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.toleranceBand, .config.minmax = {0, 200 } }, { "tolerance_band_reduction", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.toleranceBandReduction, .config.minmax = {0, 100 } }, diff --git a/src/main/mw.c b/src/main/mw.c index 91165131db..2848abd40e 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -518,6 +518,12 @@ void processRx(void) This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air */ if (throttleStatus == THROTTLE_LOW && !wasAirmodeIsActivated) { pidResetErrorGyroState(); + if (currentProfile->pidProfile.zeroThrottleStabilisation) + pidStabilisationState(PID_STABILISATION_ON); + else + pidStabilisationState(PID_STABILISATION_OFF); + } else { + if (currentProfile->pidProfile.zeroThrottleStabilisation || wasAirmodeIsActivated) pidStabilisationState(PID_STABILISATION_ON); } // When armed and motors aren't spinning, do beeps and then disarm