Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 00:05:28 +03:00
Code Issues Wiki Activity

TPA calculation fixes; Force re-calculation of PIDs when they are changed by MSP or adjustments

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2016-10-17 11:34:40 +10:00
parent ade14763a5
commit fe8156be2a
4 changed files with 34 additions and 6 deletions
Download patch file Download diff file Expand all files Collapse all files

3
src/main/fc/fc_msp.c
View file

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

12
src/main/fc/rc_controls.c
View file

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

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

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

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

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