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

Fix bugs in gain scaling; Allow gain scaling for NAV PID

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2017-09-05 23:52:42 +10:00
parent 89e86bbc8b
commit 8666edc80e
3 changed files with 24 additions and 15 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1330,13 +1330,13 @@ static void navProcessFSMEvents(navigationFSMEvent_t injectedEvent)
// Implementation of PID with back-calculation I-term anti-windup
// Control System Design, Lecture Notes for ME 155A by Karl Johan Åström (p.228)
// http://www.cds.caltech.edu/~murray/courses/cds101/fa02/caltech/astrom-ch6.pdf
float navPidApply2(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags)
float navPidApply3(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags, const float gainScaler)
{
float newProportional, newDerivative;
float error = setpoint - measurement;
/* P-term */
newProportional = error * pid->param.kP;
newProportional = error * pid->param.kP * gainScaler;
/* D-term */
if (pidFlags & PID_DTERM_FROM_ERROR) {
@ -1350,19 +1350,19 @@ float navPidApply2(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);
newDerivative = pid->param.kD * pt1FilterApply4(&pid->dterm_filter_state, newDerivative, NAV_DTERM_CUT_HZ, dt) * gainScaler;
if (pidFlags & PID_ZERO_INTEGRATOR) {
pid->integrator = 0.0f;
}
/* Pre-calculate output and limit it if actuator is saturating */
const float outVal = newProportional + pid->integrator + newDerivative;
const float outVal = newProportional + (pid->integrator * gainScaler) + newDerivative;
const float outValConstrained = constrainf(outVal, outMin, outMax);
/* Update I-term */
if (!(pidFlags & PID_ZERO_INTEGRATOR)) {
const float newIntegrator = pid->integrator + (error * pid->param.kI * dt) + ((outValConstrained - outVal) * pid->param.kT * dt);
const float newIntegrator = pid->integrator + (error * pid->param.kI * gainScaler * dt) + ((outValConstrained - outVal) * pid->param.kT * dt);
if (pidFlags & PID_SHRINK_INTEGRATOR) {
// Only allow integrator to shrink
@ -1378,6 +1378,12 @@ float navPidApply2(pidController_t *pid, const float setpoint, const float measu
return outValConstrained;
}
float navPidApply2(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags)
{
return navPidApply3(pid, setpoint, measurement, dt, outMin, outMax, pidFlags, 1.0f);
}
void navPidReset(pidController_t *pid)
{
pid->integrator = 0.0f;
@ -2605,9 +2611,9 @@ void navigationUsePIDs(void)
(float)pidProfile()->bank_fw.pid[PID_POS_XY].I / 100.0f,
(float)pidProfile()->bank_fw.pid[PID_POS_XY].D / 100.0f);
navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P,
(float)pidProfile()->bank_fw.pid[PID_POS_Z].I,
(float)pidProfile()->bank_fw.pid[PID_POS_Z].D);
navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 9.80665f,
(float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 9.80665f,
(float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 9.80665f);
}
void navigationInit(void)