Tilt-compensated motor output to keep altitude for quadcopter

Question

The propellers of a multicopter produce thrust. Unfortunately the thrust is the smaller, the more the copter is tilted. I was currently wondering whether there is an established method to calculate how much the overall thrust has to be modified to hold the current altitude, based on the current attitude.

This is the way a calculate the motor output so far. rol/pit/yaw-output already ran through the PIDs.

// Calculate the speed of the motors
int_fast16_t iFL = rcthr + rol_output + pit_output - yaw_output;
int_fast16_t iBL = rcthr + rol_output - pit_output + yaw_output;
int_fast16_t iFR = rcthr - rol_output + pit_output + yaw_output;
int_fast16_t iBR = rcthr - rol_output - pit_output - yaw_output;

Answer 1

The thrust vector is given by $f = qRg$ where $q$ is the magnitude of the thrust vector, $R$ is the rotation matrix for the current attitude and $g$ is the gravity vector in world coords, i.e. $[0~0~\pm1]^T$. Expanding $R$ for the ZYX Euler angles usually used for quadrotors, we have

$$\ \begin{align} R &= \begin{bmatrix} c_\psi c_\theta & c_\psi s_\theta s_\phi - c_\phi s_\psi & s_\psi s_\phi + c_\psi c_\phi s_\theta \\ c_\theta s_\psi & c_\psi c_\phi + s_\psi s_\theta s_\phi & c_\phi s_\psi s_\theta - c_\psi s_\phi \\ -s_\theta & c_\theta s_\phi & c_\theta c_\phi \end{bmatrix} \end{align} $$ where $\phi$ is the roll angle, $\theta$ is the pitch angle, and $\psi$ is the yaw angle.

For simplicity, assume $\psi=0$ since it doesn't affect the vertical lift force. Multiplying $Rg$ and looking at just the vertical component we have $$ f_z = q c_\theta c_\phi \\ q = \frac{f_z}{c_\theta c_\phi} $$

So just plug in your desired lift force (e.q. mass*gravity) and use your current roll and pitch angles to get the needed total thrust force.