diff --git a/docs/development/PID Internals.md b/docs/development/PID Internals.md new file mode 100644 index 0000000000..19f3e70969 --- /dev/null +++ b/docs/development/PID Internals.md @@ -0,0 +1,39 @@ +### IO variables + +gyroData/8192*2000 = deg/s +gyroData/4 ~ deg/s + +rcCommand - -500-500 nominal, but is scaled with rcRate/100, max +-1250 + +inclination - in 0.1 degree, default 50 degrees (500 units) + +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] +P = constrain(P8[PIDLEVEL]/100 * error, limit +- D8[PIDLEVEL] * 5) +I = intergrate(error, limit +-10000) * I8[PIDLEVEL] / 4096 + +## 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 + +## Mode dependent mix(yaw is always from gyro) + +HORIZON - proportionally according to max deflection +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