Tom asked me in a previous comment to show diagrammatically how all the bits fit together. Here you go, Tom.
From left to right:
- the target for the horizontal speed PID is defined by user input (remote control or pre-programmed)
- the input is integrated from the accelerometer
- the output is a desired horizontal acceleration to make input and target match
- the horizontal acceleration output is converted to the angle target for the absolute angle PID by a slightly arbitrary atan2() function comparing horizontal acceleration against gravity to produce the desired target angle
- the input comes from both the integrated gyro and the Euler angles derived from the accelerometer merged together by a complementary filter
- the output provides the target for the angular rate PID
- the input comes direct from the gyro
- the output value is then used to change the PWM driving the motors ESCs to achieve the desired horizontal speed.
The resultant output is then applied to the propeller motors ESC PWM signals – so for a forward tilt, half the output is added to the rear motors, and half the output is subtracted from the front motors. The same applies to the Y axis.
The Z axis PID is hardly worth showing a diagram but it’s there at the bottom, just one PID – target is a desired vertical speed, input is integrated from the accelerometer, output is fed to all props equally.
The matrix is there to convert sensors readings from the tilted quadcopter to earth coordinates – essentially accelerometer readings get matrixed into earth axis vectors before integrating to give horizontal and vertical speeds to feed the left-most of the PIDs.