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

Make VelXY Dterm LPF user configurable

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2019-02-22 19:28:50 +01:00
parent e52b5d9480
commit ddff19f399
6 changed files with 31 additions and 11 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1655,7 +1655,11 @@ float navPidApply3(pidController_t *pid, const float setpoint, const float measu
pid->last_input = measurement;
}
newDerivative = pid->param.kD * pt1FilterApply4(&pid->dterm_filter_state, newDerivative, NAV_DTERM_CUT_HZ, dt) * gainScaler;
if (pid->dTermLpfHz > 0) {
newDerivative = pid->param.kD * pt1FilterApply4(&pid->dterm_filter_state, newDerivative, pid->dTermLpfHz, dt) * gainScaler;
} else {
newDerivative = pid->param.kD * newDerivative;
}
if (pidFlags & PID_ZERO_INTEGRATOR) {
pid->integrator = 0.0f;
@ -1714,7 +1718,7 @@ void navPidReset(pidController_t *pid)
pid->output_constrained = 0.0f;
}
void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF)
void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF, float _dTermLpfHz)
{
pid->param.kP = _kP;
pid->param.kI = _kI;
@ -1730,7 +1734,7 @@ void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kF
pid->param.kI = 0.0;
pid->param.kT = 0.0;
}
pid->dTermLpfHz = _dTermLpfHz;
navPidReset(pid);
}
@ -3038,7 +3042,8 @@ void navigationUsePIDs(void)
navPidInit(&posControl.pids.vel[axis], (float)pidProfile()->bank_mc.pid[PID_VEL_XY].P / 20.0f,
(float)pidProfile()->bank_mc.pid[PID_VEL_XY].I / 100.0f,
(float)pidProfile()->bank_mc.pid[PID_VEL_XY].D / 100.0f,
(float)pidProfile()->bank_mc.pid[PID_VEL_XY].FF / 100.0f
(float)pidProfile()->bank_mc.pid[PID_VEL_XY].FF / 100.0f,
pidProfile()->navVelXyDTermLpfHz
);
}
@ -3048,25 +3053,33 @@ void navigationUsePIDs(void)
navPidInit(&posControl.pids.vel[Z], (float)pidProfile()->bank_mc.pid[PID_VEL_Z].P / 66.7f,
(float)pidProfile()->bank_mc.pid[PID_VEL_Z].I / 20.0f,
(float)pidProfile()->bank_mc.pid[PID_VEL_Z].D / 100.0f,
0.0f);
0.0f,
NAV_DTERM_CUT_HZ
);
// Initialize surface tracking PID
navPidInit(&posControl.pids.surface, 2.0f,
0.0f,
0.0f,
0.0f);
0.0f,
NAV_DTERM_CUT_HZ
);
/** Airplane PIDs */
// Initialize fixed wing PID controllers
navPidInit(&posControl.pids.fw_nav, (float)pidProfile()->bank_fw.pid[PID_POS_XY].P / 100.0f,
(float)pidProfile()->bank_fw.pid[PID_POS_XY].I / 100.0f,
(float)pidProfile()->bank_fw.pid[PID_POS_XY].D / 100.0f,
0.0f);
0.0f,
NAV_DTERM_CUT_HZ
);
navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 10.0f,
(float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 10.0f,
(float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 10.0f,
0.0f);
0.0f,
NAV_DTERM_CUT_HZ
);
}
void navigationInit(void)