# Quadrocopter control loop: Recombining axis accelerations to motor speeds

First, as I always have to look this up myself – the following image explains yaw, pitch and roll rotations:

At some point in the quadrocopter control loop, we might have calculated that we need significantly more thrust on the left side than on the right, slightly emphasizing the front rotors to increase pitch and reducing the overall thrust a little to stop elevating.

This situation is usually modelled as four values: three dimensions for the axes and one for the overall thrust. The three values for the axes can be positive or negative, while the overall thrust is strictly positive. From this four-dimensional vector, we need to derive the motor accelerations of all four motors (strictly positive, they typically cannot spin backwards). The following calculation for a quadcopter can be done with different orientations in mind, the usual variants are “+” (one rotor in front/left/right/back) and “X” (front/left/right/back direction is always in between two rotors). Somehow I always liked the “X” configuration, so I use it, but mathematically the approaches are of course equivalent. The proof consists in turning a remote control by 45 degrees ;). The following model describes how the axis accelerations and vertical acceleration map to the individual motors:

$\begin{array}{rccccrrrr} \text{pitchAcc}&=&\text{frontLeftAcc} &+ &\text{frontRightAcc}&-&\text{rearLeftAcc}&-&\text{rearRightAcc}\\ \text{rollAcc}&= &\text{frontLeftAcc}&-&\text{frontRightAcc}&+&\text{rearLeftAcc}&-&\text{rearRightAcc}\\ \text{yawAcc}&= &\text{frontLeftAcc}&-&\text{frontRightAcc}&-&\text{rearLeftAcc}&+&\text{rearRightAcc}\\ \text{upAcc}&= &\text{frontLeftAcc}&+&\text{frontRightAcc}&+&\text{rearLeftAcc}&+&\text{rearRightAcc}\\ \end{array}$

A quadrocopter can turn to the right (yaw) because opposite motors are set up to rotate in the same direction, different from the other two. If the clockwise rotation motors turn faster than the anticlockwise motors, the quadrocopter turns anti-clockwise. So the yawAcc equation might be different (multiply one side with -1) for a different motor setup.

Because at our position in the control loop, the vector (pitchAcc, rollAcc, yawAcc, upAcc) is given for the current timestep, we need to rearrange the equations above to calculate the individual motor thrusts:

$\left( \begin{array}{c} \text{frontLeft} \\ \text{frontRight}\\ \text{rearLeft}\\ \text{rearRight}\\ \end{array} \right) = \frac{1}{4} \left[ \left( \begin{array}{c} \text{upAcc} \\ \text{upAcc}\\ \text{upAcc}\\ \text{upAcc}\\ \end{array} \right) + \left( \begin{array}{r} \text{pitchAcc} \\ \text{pitchAcc}\\ \text{-pitchAcc}\\ \text{-pitchAcc}\\ \end{array} \right) + \left( \begin{array}{r} \text{rollAcc} \\ \text{-rollAcc}\\ \text{rollAcc}\\ \text{-rollAcc}\\ \end{array} \right)+ \left( \begin{array}{r} \text{yawAcc} \\ \text{-yawAcc}\\ \text{-yawAcc}\\ \text{yawAcc}\\ \end{array} \right)\right]$

This is what we need! We now have a simple linear transformation from the four accelerations to four motor powers. The factor $\frac{1}{4}$ on the right side is often omitted because we get our inputs from PIDs, whose constants have to be tuned. If we leave out the constant factor here, the final PID constants will compensate for it.

Advertisements