diff --git a/docs/INAV PID Controller.md b/docs/INAV PID Controller.md new file mode 100644 index 0000000000..7bf5945e48 --- /dev/null +++ b/docs/INAV PID 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 \ No newline at end of file diff --git a/docs/development/PID Internals.md b/docs/development/PID Internals.md deleted file mode 100644 index 6cb81db68f..0000000000 --- a/docs/development/PID Internals.md +++ /dev/null @@ -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 `` (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