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

Merge pull request #3327 from shellixyz/fix_althold_initial_climb

Browse source 
Fix ALTHOLD initial climb when enabled
This commit is contained in:
Alberto García Hierro 2018-06-07 19:20:25 +01:00 • committed by GitHub
commit b67d73aeac
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
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;