Move anti-windup iterm reset from rx loop to pid loop
Having the iterm resetting happening in the rx loop causes a sawtooth PID/motor effect while idling since the PID loop is running at a much higher rate and iterm is allowed to grow during this, and then only reset at a much lower rate in the rx loop. This can potentially lead to some oscillation and/or resonance while idling before takeoff as the sawtooth signal can make it through to the motor outputs.
This commit is contained in:
parent
628fdb8adc
commit
d867aeced3
|
@ -676,12 +676,13 @@ bool processRx(timeUs_t currentTimeUs)
|
||||||
/* In airmode iterm should be prevented to grow when Low thottle and Roll + Pitch Centered.
|
/* 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 */
|
This is needed to prevent iterm winding on the ground, but keep full stabilisation on 0 throttle while in air */
|
||||||
if (throttleStatus == THROTTLE_LOW && !airmodeIsActivated && !launchControlActive) {
|
if (throttleStatus == THROTTLE_LOW && !airmodeIsActivated && !launchControlActive) {
|
||||||
pidResetIterm();
|
pidSetItermReset(true);
|
||||||
if (currentPidProfile->pidAtMinThrottle)
|
if (currentPidProfile->pidAtMinThrottle)
|
||||||
pidStabilisationState(PID_STABILISATION_ON);
|
pidStabilisationState(PID_STABILISATION_ON);
|
||||||
else
|
else
|
||||||
pidStabilisationState(PID_STABILISATION_OFF);
|
pidStabilisationState(PID_STABILISATION_OFF);
|
||||||
} else {
|
} else {
|
||||||
|
pidSetItermReset(false);
|
||||||
pidStabilisationState(PID_STABILISATION_ON);
|
pidStabilisationState(PID_STABILISATION_ON);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -80,6 +80,7 @@ static FAST_RAM_ZERO_INIT float antiGravityThrottleHpf;
|
||||||
static FAST_RAM_ZERO_INIT uint16_t itermAcceleratorGain;
|
static FAST_RAM_ZERO_INIT uint16_t itermAcceleratorGain;
|
||||||
static FAST_RAM float antiGravityOsdCutoff = 1.0f;
|
static FAST_RAM float antiGravityOsdCutoff = 1.0f;
|
||||||
static FAST_RAM_ZERO_INIT bool antiGravityEnabled;
|
static FAST_RAM_ZERO_INIT bool antiGravityEnabled;
|
||||||
|
static FAST_RAM_ZERO_INIT bool zeroThrottleItermReset;
|
||||||
|
|
||||||
PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 2);
|
PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 2);
|
||||||
|
|
||||||
|
@ -1392,6 +1393,8 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
|
||||||
|
|
||||||
pidData[axis].Sum = 0;
|
pidData[axis].Sum = 0;
|
||||||
}
|
}
|
||||||
|
} else if (zeroThrottleItermReset) {
|
||||||
|
pidResetIterm();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -1444,3 +1447,8 @@ void dynLpfDTermUpdate(float throttle)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
void pidSetItermReset(bool enabled)
|
||||||
|
{
|
||||||
|
zeroThrottleItermReset = enabled;
|
||||||
|
}
|
||||||
|
|
|
@ -232,4 +232,4 @@ float pidLevel(int axis, const pidProfile_t *pidProfile,
|
||||||
float calcHorizonLevelStrength(void);
|
float calcHorizonLevelStrength(void);
|
||||||
#endif
|
#endif
|
||||||
void dynLpfDTermUpdate(float throttle);
|
void dynLpfDTermUpdate(float throttle);
|
||||||
|
void pidSetItermReset(bool enabled);
|
||||||
|
|
|
@ -1088,4 +1088,5 @@ extern "C" {
|
||||||
bool gpsIsHealthy() { return false; }
|
bool gpsIsHealthy() { return false; }
|
||||||
bool isAltitudeOffset(void) { return false; }
|
bool isAltitudeOffset(void) { return false; }
|
||||||
float getCosTiltAngle(void) { return 0.0f; }
|
float getCosTiltAngle(void) { return 0.0f; }
|
||||||
|
void pidSetItermReset(bool) {}
|
||||||
}
|
}
|
||||||
|
|
|
@ -174,4 +174,5 @@ extern "C" {
|
||||||
bool usbVcpIsConnected(void) { return false; }
|
bool usbVcpIsConnected(void) { return false; }
|
||||||
void pidSetAntiGravityState(bool newState) { UNUSED(newState); }
|
void pidSetAntiGravityState(bool newState) { UNUSED(newState); }
|
||||||
void osdSuppressStats(bool) {}
|
void osdSuppressStats(bool) {}
|
||||||
|
void pidSetItermReset(bool) {}
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue