diff --git a/docs/development/PID Internals.md b/docs/development/PID Internals.md index 19f3e70969..a4ed3fce28 100644 --- a/docs/development/PID Internals.md +++ b/docs/development/PID Internals.md @@ -1,39 +1,46 @@ ### IO variables -gyroData/8192*2000 = deg/s -gyroData/4 ~ deg/s +`gyroData/8192*2000 = deg/s` -rcCommand - -500-500 nominal, but is scaled with rcRate/100, max +-1250 +`gyroData/4 ~ deg/s` -inclination - in 0.1 degree, default 50 degrees (500 units) +`rcCommand` - `<-500 - 500>` nominal, but is scaled with `rcRate/100`, max +-1250 -axisPID - output to mixer, will be added to throttle(1000-2000), output range is (default <1150 - 1850>) +`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 - +#### Leveling term +``` error = constrain(2*rcCommand[axis], limit +- max_angle_inclination) - inclination[axis] -P = constrain(P8[PIDLEVEL]/100 * error, limit +- D8[PIDLEVEL] * 5) +P = constrain(P8[PIDLEVEL]/100 * error, limit +- 5 * D8[PIDLEVEL]) I = intergrate(error, limit +-10000) * I8[PIDLEVEL] / 4096 - -## Gyro term - +``` +#### Gyro term +``` P = rcCommand[axis]; error = rcCommand[axis] * 10 * 8 / pidProfile->P8[axis] - gyroData[axis] / 4; (conversion so that error is in deg/s ?) I = 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 + - axis rotation rate > +-64deg/s + - axis is YAW and rcCommand>+-100 -## Mode dependent mix(yaw is always from gyro) +##### Mode dependent mix(yaw is always from gyro) -HORIZON - proportionally according to max deflection -Gyro - gyro term -Angle - leveling term +- HORIZON - proportionally according to max deflection -## Gyro stabilization +- gyro - gyro term + +- ANGLE - leveling term + +#### Gyro stabilization + +``` P = -gyroData[axis] / 4 * dynP8 / 10 / 8 -D = -mean(diff(gyroData[axis] / 4), 3) * 3 * dynP8 / 32 \ No newline at end of file +D = -mean(diff(gyroData[axis] / 4), over 3 samples) * 3 * dynP8 / 32 +``` \ No newline at end of file