diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 64fad49d85..290ffd86d3 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1131,6 +1131,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) break; case MSP_SET_PID: + signalRequiredPIDCoefficientsUpdate(); for (i = 0; i < PID_ITEM_COUNT; i++) { currentProfile->pidProfile.P8[i] = sbufReadU8(src); currentProfile->pidProfile.I8[i] = sbufReadU8(src); @@ -1200,6 +1201,8 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) if (dataSize >= 11) { currentControlRateProfile->rcYawExpo8 = sbufReadU8(src); } + + signalRequiredPIDCoefficientsUpdate(); } else { return MSP_RESULT_ERROR; } diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 6557a80654..a5d98ee35a 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -527,6 +527,7 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm controlRateConfig->rates[FD_PITCH] = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_RATE, newValue); if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) { + signalRequiredPIDCoefficientsUpdate(); break; } // follow though for combined ADJUSTMENT_PITCH_ROLL_RATE @@ -534,11 +535,13 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm newValue = constrain((int)controlRateConfig->rates[FD_ROLL] + delta, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); controlRateConfig->rates[FD_ROLL] = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_RATE, newValue); + signalRequiredPIDCoefficientsUpdate(); break; case ADJUSTMENT_YAW_RATE: newValue = constrain((int)controlRateConfig->rates[FD_YAW] + delta, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX); controlRateConfig->rates[FD_YAW] = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_RATE, newValue); + signalRequiredPIDCoefficientsUpdate(); break; case ADJUSTMENT_PITCH_ROLL_P: case ADJUSTMENT_PITCH_P: @@ -546,6 +549,7 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm pidProfile->P8[PIDPITCH] = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_P, newValue); if (adjustmentFunction == ADJUSTMENT_PITCH_P) { + signalRequiredPIDCoefficientsUpdate(); break; } // follow though for combined ADJUSTMENT_PITCH_ROLL_P @@ -553,6 +557,7 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm newValue = constrain((int)pidProfile->P8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c pidProfile->P8[PIDROLL] = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_P, newValue); + signalRequiredPIDCoefficientsUpdate(); break; case ADJUSTMENT_PITCH_ROLL_I: case ADJUSTMENT_PITCH_I: @@ -560,6 +565,7 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm pidProfile->I8[PIDPITCH] = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_I, newValue); if (adjustmentFunction == ADJUSTMENT_PITCH_I) { + signalRequiredPIDCoefficientsUpdate(); break; } // follow though for combined ADJUSTMENT_PITCH_ROLL_I @@ -567,6 +573,7 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm newValue = constrain((int)pidProfile->I8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c pidProfile->I8[PIDROLL] = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_I, newValue); + signalRequiredPIDCoefficientsUpdate(); break; case ADJUSTMENT_PITCH_ROLL_D: case ADJUSTMENT_PITCH_D: @@ -574,6 +581,7 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm pidProfile->D8[PIDPITCH] = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_D, newValue); if (adjustmentFunction == ADJUSTMENT_PITCH_D) { + signalRequiredPIDCoefficientsUpdate(); break; } // follow though for combined ADJUSTMENT_PITCH_ROLL_D @@ -581,21 +589,25 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm newValue = constrain((int)pidProfile->D8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c pidProfile->D8[PIDROLL] = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_D, newValue); + signalRequiredPIDCoefficientsUpdate(); break; case ADJUSTMENT_YAW_P: newValue = constrain((int)pidProfile->P8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c pidProfile->P8[PIDYAW] = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_P, newValue); + signalRequiredPIDCoefficientsUpdate(); break; case ADJUSTMENT_YAW_I: newValue = constrain((int)pidProfile->I8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c pidProfile->I8[PIDYAW] = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_I, newValue); + signalRequiredPIDCoefficientsUpdate(); break; case ADJUSTMENT_YAW_D: newValue = constrain((int)pidProfile->D8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c pidProfile->D8[PIDYAW] = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_D, newValue); + signalRequiredPIDCoefficientsUpdate(); break; default: break; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 8d124c473a..a02e569526 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -91,6 +91,7 @@ int16_t magHoldTargetHeading; static pt1Filter_t magHoldRateFilter; // Thrust PID Attenuation factor. 0.0f means fully attenuated, 1.0f no attenuation is applied +static bool shouldUpdatePIDCoeffs = false; static float tpaFactor; int16_t axisPID[FLIGHT_DYNAMICS_INDEX_COUNT]; @@ -160,19 +161,23 @@ FP-PID has been rescaled to match LuxFloat (and MWRewrite) from Cleanflight 1.13 #define FP_PID_LEVEL_P_MULTIPLIER 65.6f #define FP_PID_YAWHOLD_P_MULTIPLIER 80.0f +void signalRequiredPIDCoefficientsUpdate(void) +{ + shouldUpdatePIDCoeffs = true; +} + void updatePIDCoefficients(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig, const struct motorConfig_s *motorConfig) { static uint16_t prevThrottle = 0; - bool shouldUpdateCoeffs = false; // Check if throttle changed if (rcCommand[THROTTLE] != prevThrottle) { prevThrottle = rcCommand[THROTTLE]; - shouldUpdateCoeffs = true; + signalRequiredPIDCoefficientsUpdate(); } // If nothing changed - don't waste time recalculating coefficients - if (!shouldUpdateCoeffs) { + if (!shouldUpdatePIDCoeffs) { return; } @@ -180,9 +185,14 @@ void updatePIDCoefficients(const pidProfile_t *pidProfile, const controlRateConf if (STATE(FIXED_WING)) { // tpa_rate is ignored for fixed wings, set to non-zero to enable TPA // tpa_breakpoint for fixed wing is cruise throttle value (value at which PIDs were tuned) - if (controlRateConfig->dynThrPID != 0 && rcCommand[THROTTLE] > motorConfig->minthrottle && controlRateConfig->tpa_breakpoint > motorConfig->minthrottle) { - tpaFactor = 0.5f + ((controlRateConfig->tpa_breakpoint - motorConfig->minthrottle) / (rcCommand[THROTTLE] - motorConfig->minthrottle) / 2.0f); - tpaFactor = constrainf(tpaFactor, 0.6f, 1.67f); + if (controlRateConfig->dynThrPID != 0 && controlRateConfig->tpa_breakpoint > motorConfig->minthrottle) { + if (rcCommand[THROTTLE] > motorConfig->minthrottle) { + tpaFactor = 0.5f + ((controlRateConfig->tpa_breakpoint - motorConfig->minthrottle) / (rcCommand[THROTTLE] - motorConfig->minthrottle) / 2.0f); + tpaFactor = constrainf(tpaFactor, 0.6f, 1.67f); + } + else { + tpaFactor = 1.67f; + } } else { tpaFactor = 1.0f; @@ -227,6 +237,8 @@ void updatePIDCoefficients(const pidProfile_t *pidProfile, const controlRateConf pidState[axis].kT = 0; } } + + shouldUpdatePIDCoeffs = false; } static void pidApplyHeadingLock(const pidProfile_t *pidProfile, pidState_t *pidState) diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index b36de645f5..6cc535e576 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -85,6 +85,7 @@ struct controlRateConfig_s; struct motorConfig_s; struct rxConfig_s; +void signalRequiredPIDCoefficientsUpdate(void); void updatePIDCoefficients(const pidProfile_t *pidProfile, const struct controlRateConfig_s *controlRateConfig, const struct motorConfig_s *motorConfig); void pidController(const pidProfile_t *pidProfile, const struct controlRateConfig_s *controlRateConfig, const struct rxConfig_s *rxConfig);