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

Fix ALTHOLD initial climb when enabled

Browse source 
Makes the transition to ALTHOLD seamless. Tested on fixed wing.
This commit is contained in:
Michel Pastor 2018-06-06 01:08:57 +02:00
parent 50847b9224
commit 1801d9b7eb
2 changed files with 8 additions and 2 deletions
Download patch file Download diff file Expand all files Collapse all files

9
src/main/navigation/navigation.c
View file

@ -1324,12 +1324,16 @@ float navPidApply3(pidController_t *pid, const float setpoint, const float measu
newProportional = error * pid->param.kP * gainScaler;
/* D-term */
if (pid->reset) {
pid->last_input = (pidFlags & PID_DTERM_FROM_ERROR) ? error : measurement;
pid->reset = false;
}
if (pidFlags & PID_DTERM_FROM_ERROR) {
/* Error-tracking D-term */
newDerivative = (error - pid->last_input) / dt;
pid->last_input = error;
}
else {
} else {
/* Measurement tracking D-term */
newDerivative = -(measurement - pid->last_input) / dt;
pid->last_input = measurement;
@ -1371,6 +1375,7 @@ float navPidApply2(pidController_t *pid, const float setpoint, const float measu
void navPidReset(pidController_t *pid)
{
pid->reset = true;
pid->integrator = 0.0f;
pid->last_input = 0.0f;
pid->dterm_filter_state.state = 0.0f;