INAV PID Controller basic description

2025-07-12 19:10:27 +03:00 · 2020-09-04 13:09:42 +02:00 · 2020-09-04 13:09:42 +02:00 · cc159269d2
commit cc159269d2
parent 5d221d3a8c
2 changed files with 25 additions and 63 deletions
--- a/Controller.md
+++ b/Controller.md
@ -0,0 +1,25 @@
+# INAV PID Controller
+
+What you have to know about INAV PID/PIFF/PIDCD controllers:
+
+1. INAV PID uses floating-point math
+1. Rate/Angular Velocity controllers work in dps [degrees per second]
+1. P, I, D and Multirotor CD gains are scaled like Betafligfht equivalents, but actual mechanics are different, and PID response might be different
+1. Depending on platform type, different controllers are used
+    1. Fixed-wing uses **PIFF**:
+        1. Error is computed with a formula `const float rateError = pidState->rateTarget - pidState->gyroRate;`
+        1. P-term with a formula `rateError * pidState->kP`
+        1. Simple I-term without Iterm Relax. I-term limit based on stick position is used instead. I-term is no allowed to grow if stick (roll/pitch/yaw) is deflected above threshold defined in `fw_iterm_limit_stick_position`. `pidState->errorGyroIf += rateError * pidState->kI * dT;`
+        1. No D-term
+        1. FF-term (Feed Forward) is computed from the controller input with a formula `pidState->rateTarget * pidState->kFF`. Bear in mind, this is not a **FeedForward** from Betaflight!
+    1. Multirotor uses **PIDCD**:
+        1.  Error is computed with a formula `const float rateError = pidState->rateTarget - pidState->gyroRate;`
+        1. P-term with a formula `rateError * pidState->kP`
+        1. I-term
+            1. Iterm Relax is used to dynamically attenuate I-term during fast stick movements
+            1. I-term formula `pidState->errorGyroIf += (itermErrorRate * pidState->kI * antiWindupScaler * dT) + ((newOutputLimited - newOutput) * pidState->kT * antiWindupScaler * dT);`
+            1. I-term can be limited when motor output is saturated
+        1. D-term is computed only from gyro measurement
+        1. There are 2 LPF filters on D-term 
+        1. D-term can by boosted during fast maneuvers using D-Boost. D-Boost is an equivalent of Betaflight D_min
+        1. **Control Derivative**, CD, or CD-term is a derivative computed from the setpoint that helps to boost PIDCD controller during fast stick movements. `newCDTerm = rateTargetDeltaFiltered * (pidState->kCD / dT);` It is an equivalent of Betaflight Feed Forward 
--- a/docs/development/PID
+++ b/docs/development/PID
@ -1,63 +0,0 @@
-### IO variables
-
-`gyroADC/8192*2000 = deg/s`
-
-`gyroADC/4 ~ deg/s`
-
-`rcCommand` - `<-500 - 500>` nominal, but is scaled with `rcRate/100`, max +-1250
-
-`inclination` - in 0.1 degree, roll and pitch deviation from horizontal position
-`max_angle_inclination` - in 0.1 degree, default 50 degrees (500)
-
-`axisPID` - output to mixer, will be added to throttle(`<1000-2000>`), output range is `<minthrottle, maxthrottle>` (default `<1150 - 1850>`)
-
-### PID controller 0, "MultiWii" (default)
-
-
-#### Leveling term
-```
-error = constrain(2*rcCommand[axis], limit +- max_angle_inclination) - inclination[axis]
-Pacc = constrain(P8[PIDLEVEL]/100 * error, limit +- 5 * D8[PIDLEVEL])
-Iacc = intergrate(error, limit +-10000) * I8[PIDLEVEL] / 4096
-```
-#### Gyro term
-```
-Pgyro = rcCommand[axis];
-error = rcCommand[axis] * 10 * 8 / pidProfile->P8[axis] - gyroADC[axis] / 4; (conversion so that error is in deg/s ?)
-Igyro = integrate(error, limit +-16000) / 10 / 8  * I8[axis] / 100 (conversion back to mixer units ?)
-```
-
-reset I term if
-  - axis rotation rate > +-64deg/s
-  - axis is YAW and rcCommand>+-100
-
-##### Mode dependent mix(yaw is always from gyro)
-
- HORIZON - proportionally according to max deflection
-```
-  deflection = MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])) / 500 ; limit to 0.0 .. 1.0
-  P = Pacc * (1-deflection) + Pgyro * deflection
-  I = Iacc * (1-deflection) + Igyro * deflection
-```
- gyro
-```
-  P = Pgyro
-  I = Igyro
-```
- ANGLE
-```
-  P = Pacc
-  I = Iacc
-```
-#### Gyro stabilization
-
-```
-P -=  gyroADC[axis] / 4 * dynP8 / 10 / 8
-D = -mean(diff(gyroADC[axis] / 4), over 3 samples) * 3 * dynD8 / 32
-[equivalent to :]
-D = - (gyroADC[axis]/4 - (<3 loops old>gyroADC[axis]/4)) * dynD8 / 32
-```
-
-This can be seen as sum of
- - PI controller (handles rcCommand, HORIZON/ANGLE); `Igyro` is only output based on gyroADC
- - PD controller(parameters dynP8/dynD8) with zero setpoint acting on gyroADC