Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 01:35:35 +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; newProportional = error * pid->param.kP * gainScaler;
/* D-term */ /* D-term */
if (pid->reset) {
pid->last_input = (pidFlags & PID_DTERM_FROM_ERROR) ? error : measurement;
pid->reset = false;
}
if (pidFlags & PID_DTERM_FROM_ERROR) { if (pidFlags & PID_DTERM_FROM_ERROR) {
/* Error-tracking D-term */ /* Error-tracking D-term */
newDerivative = (error - pid->last_input) / dt; newDerivative = (error - pid->last_input) / dt;
pid->last_input = error; pid->last_input = error;
} } else {
else {
/* Measurement tracking D-term */ /* Measurement tracking D-term */
newDerivative = -(measurement - pid->last_input) / dt; newDerivative = -(measurement - pid->last_input) / dt;
pid->last_input = measurement; pid->last_input = measurement;
@ -1371,6 +1375,7 @@ float navPidApply2(pidController_t *pid, const float setpoint, const float measu
void navPidReset(pidController_t *pid) void navPidReset(pidController_t *pid)
{ {
pid->reset = true;
pid->integrator = 0.0f; pid->integrator = 0.0f;
pid->last_input = 0.0f; pid->last_input = 0.0f;
pid->dterm_filter_state.state = 0.0f; pid->dterm_filter_state.state = 0.0f;

1
src/main/navigation/navigation_private.h
View file

@ -107,6 +107,7 @@ typedef enum {
} pidControllerFlags_e; } pidControllerFlags_e;
typedef struct { typedef struct {
bool reset;
pidControllerParam_t param; pidControllerParam_t param;
pt1Filter_t dterm_filter_state; // last derivative for low-pass filter pt1Filter_t dterm_filter_state; // last derivative for low-pass filter
float integrator; // integrator value float integrator; // integrator value