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

Attenuate VEL_XY when UAV is traveling at high speed

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2020-01-19 20:50:39 +01:00
parent 608d08cfef
commit f30a212378
7 changed files with 107 additions and 18 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1711,8 +1711,17 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode)
// 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 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 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,
const float dTermScaler
) {
float newProportional, newDerivative, newFeedForward;
float error = setpoint - measurement;
@ -1736,11 +1745,13 @@ float navPidApply3(pidController_t *pid, const float setpoint, const float measu
}
if (pid->dTermLpfHz > 0) {
newDerivative = pid->param.kD * pt1FilterApply4(&pid->dterm_filter_state, newDerivative, pid->dTermLpfHz, dt) * gainScaler;
newDerivative = pid->param.kD * pt1FilterApply4(&pid->dterm_filter_state, newDerivative, pid->dTermLpfHz, dt);
} else {
newDerivative = pid->param.kD * newDerivative;
}
newDerivative = newDerivative * gainScaler * dTermScaler;
if (pidFlags & PID_ZERO_INTEGRATOR) {
pid->integrator = 0.0f;
}
@ -1780,7 +1791,7 @@ float navPidApply3(pidController_t *pid, const float setpoint, const float measu
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);
return navPidApply3(pid, setpoint, measurement, dt, outMin, outMax, pidFlags, 1.0f, 1.0f);
}
void navPidReset(pidController_t *pid)