On-board controller - Thrust and Torque

[magalhaesw]

Dear Group,
Please, Id like to ask for some guidance.
I am working on developing an on-board LQR. I looked at controller_mellinger.c as reference.

In simulation, the CF2 works fine, but in practice, my CF2 takes off and flips over.

It seems that I am missing some parameter related to the torque (Mx, My, Mz).

Let me explain what I have done so far:
The controller is a full-state controller with the state defined as follows,

$$ X = \begin{bmatrix} x, \ y, \ z, \ \phi, \ \theta, \ \psi, \ \dot{x}, \ \dot{y}, \ \dot{z}, \ p, \ q, \ r \end{bmatrix}^\mathrm{T} $$

And the input :

$$ u = \begin{bmatrix} Fthrust, \ Mx, \ My, \ Mz \end{bmatrix}^\mathrm{T} = k \star \Delta X + \begin{bmatrix} mg, \ 0, \ 0, \ 0 \end{bmatrix}^\mathrm{T}$$

The linearization of the quadrotor dynamics over the equilibrium point X0=[x, y, z, 0, 0, 0, 0, 0, 0, 0, 0, 0] and u0=[mg, 0, 0, 0] is :

$$A = \begin{bmatrix} 0. & 0. & 0. & 0. & 0. & 0. & 1. & 0. & 0. &, 0. & 0. & 0.\\ 0. & 0. & 0. & 0. & 0. & 0. & 0. & 1. & 0. & 0. & 0. & 0. \\ 0. & 0. & 0. & 0. & 0. & 0. & 0. & 0. & 1. & 0. & 0. & 0. \\ 0. & 0. & 0. & 0. & 0. & 0. & 0. & 0. & 0. & 1. & 0. & 0. \\ 0. & 0. & 0. & 0. & 0. & 0. & 0. & 0. & 0. & 0. & 1. & 0. \\ 0. & 0. & 0. & 0. & 0. & 0. & 0. & 0. & 0. & 0. & 0. & 1. \\ 0. & 0. & 0. & 0. & -g & 0. & 0. & 0. & 0. & 0. & 0. & 0. \\ 0. & 0. & 0. & g & 0. & 0. & 0. & 0. & 0. & 0. & 0. & 0. \\ 0. & 0. & 0. & 0. & 0. & 0. & 0. & 0. & 0. & 0. & 0. & 0. \\ 0. & 0. & 0. & 0. & 0. & 0. & 0. & 0. & 0. & 0. & 0. & 0. \\ 0. & 0. & 0. & 0. & 0. & 0. & 0. & 0. & 0. & 0. & 0. & 0. \\ 0. & 0. & 0. & 0. & 0. & 0. & 0. & 0. & 0. & 0. & 0. & 0. \\ \end{bmatrix}$$

$$B = \begin{bmatrix} 0. & 0. & 0. & 0. \\ 0. & 0. & 0. & 0. \\ 0. & 0. & 0. & 0. \\ 0. & 0. & 0. & 0. \\ 0. & 0. & 0. & 0. \\ 0. & 0. & 0. & 0. \\ 0. & 0. & 0. & 0. \\ 0. & 0. & 0. & 0 \\ \frac{1}{m} & 0. & 0. & 0. \\ 0. & \frac{1}{Ix} & 0. & 0. \\ 0. & 0. & \frac{1}{Iy} & 0. \\ 0. & 0. & 0. & \frac{1}{Iz} \\ \end{bmatrix}$$

The gain K is given by K = ARE(A,B,Q,R).
Simulating the CF2 in python, the system converges to the hover condition. The figure below shows the value of [u1, u2, u3, u4] in simulation.

The absolute values of Mx, My, Mz are smaller than 1.0.

I used the matrix K to implement the on-board controller in CF2.
In my C file, I added:

control->controlMode = controlModeLegacy ;
control->thrust = massThrust * u0 ;
control->roll = u1 ;
control->pitch = u2 ;
control->yaw = u3 ;

But my CF2 takes off and flips over.

Please,
Could you help me on this situation?
What could be wrong with my implementation?

I realized that the values of torque (Mx, My, Mz) are very low. Maybe I should multiply them with some parameter.

Thanks for all help.
Jose

[hmllr]

I am not 100% sure - but in the Mellinger controller a PID (https://github.com/bitcraze/crazyflie-firmware/blob/2022.12/src/modules/src/controller_mellinger.c#L287) with P values >50000 is used, so I strongly doubt the final values should be so small. You could always log them to make sure. So maybe there is a scaling issue in your gain matrix?
If that doesn't help, @whoenig might be able to help you, he wrote the mellinger controller on the Crazyflie.

[magalhaesw]

Thanks Hanna for your comment !

When I use Mellinger controller on my experiment, the CF2 flies as expected and the values of Mx, My, and Mz are much higher than 1.0.
Hope @whoening can help me on this case.

Please,
Let me ask you a question.
On file https://github.com/bitcraze/crazyflie-firmware/blob/master/src/modules/interface/stabilizer_types.h#L193
It is defined the ControlMode.

I am using controlModeLegacy on my experiment.
If I use ControlModeForceTorque, How do I set the values of Mx, My and Mz?

Is the code below correct?

control->controlMode = controlModeForceTorque;
control->thrustSi = u0 ;
control->torque.torqueX  = u1 ;
control->torque.torqueY = u2 ;
control->torque.torqueZ = u3 ;

Thanks for your explanation.

[krichardsson]

The control_t uses anonymous unions which is a way to reduce the amount of code needed to access a member, a sort of shorthand. The drawback is that it is not as clear. To use the torqueX member you simply use control->torqueX. Your example would be:

control->controlMode = controlModeForceTorque;
control->thrustSi = u0 ;
control->torqueX  = u1 ;
control->torqueY = u2 ;
control->torqueZ = u3 ;

You can also use the torque member to access the same data, for instance control->torqueY is equal to control->torque[1] and you could do

control->torque[0] = u1 ;
control->torque[1] = u2 ;
control->torque[2] = u3 ;

[magalhaesw]

Hi Folks, Good Afternoon!

Please, I have another question.
To set the values of moments and thrust, I use the code:

control->thrust = massThrust * u0 ;
control->roll = u1 ;
control->pitch = u2 ;
control->yaw = u3 ;

If Id like to set the $\omega_i$, i.e. the angular velocity of each propeller,
How can I code it? Is it possible to do it using the c-file "controller.c" ?

something like :

control->w1 = omega1;
control->w2 = omega2 ;
control->w3 = omega3 ;
control->w4 = omega4 ;

Thanks for your explanation.

Best,
Jose

[krichardsson]

Hi, this question is slightly too far from the title. Please start a new discussion.

[magalhaesw]

Hi @krichardsson , I will open a new discussion.
Thanks for your comment.

[magalhaesw]

Hi Folks,
I am struggling to make my LQR works.

Please,
Could you help me with few questions about the Mellinger controller?

For the Mellinger controller, it is used the parameter mass_thrust = 132,000.

Where does this number come from ?
Is the mass_thrust = 1/4KF ?

About the Mellinger paper:

Studying the Mellinger Paper, In the equation (2) we have the matrix

$$\begin{bmatrix} K_F & K_F & K_F&K_F\\ 0 & K_{F}*L & 0 & -K_{F}*L\\ -K_{F}*L & 0 & K_{F}*L & 0\\ K_{M} & -K_{M} & K_{M} & -K_{M}\\ \end{bmatrix}$$

The inverse of this matrix is,

$$\begin{bmatrix} \frac{1}{4K_F} & -\frac{\sqrt{2}}{4LK_F} & -\frac{\sqrt{2}}{4LK_F} & -\frac{1}{4K_M}\\ \frac{1}{4K_F} & -\frac{\sqrt{2}}{4LK_F} & \frac{\sqrt{2}}{4LK_F} & \frac{1}{4K_M}\\ \frac{1}{4K_F} & \frac{\sqrt{2}}{4LK_F} & \frac{\sqrt{2}}{4LK_F} & -\frac{1}{4K_M}\\ \frac{1}{4K_F} & \frac{\sqrt{2}}{4LK_F} & -\frac{\sqrt{2}}{4LK_F} & \frac{1}{4K_M}\\ \end{bmatrix}$$

1. Is the mass_thrust = 1/4KF ?
2. If so, What are the values KF, KM and L used in the implementation of Mellinger controller ?

Thanks for your help.

[whoenig]

All controllers currently do not use SI units, so mass_thrust is an arbitrary, tuned value that maps to PWM, not force. None of the controllers is concerned with the matrices you provide - these are part of the power distribution.

[magalhaesw]

Thanks @whoenig for your answer.

Please,
Could you clarify one more thing?

Thrust, Mx, My, Mz are directly related to the angular velocity of the propellers (PWM).

Since for the Mellinger controller we have to multiply the Thrust to mass_thrust =132,000,
I believe that I have to do similar thing with Mx, My and Mz to my LQR Controller, i.e. , I might have to use multipliers that map the torques to PWM ( Lets call them mass_torque_x, mass_torque_y, mass_torque_z ).

Is this assumption correct?

For my LQR controller, the values of Mx, My and Mz are very small (just like the "pure" Thrust).
So I do believe that I have to multiply these torques to a tuned value that maps to PWM.

In your implementation of Mellinger controller, you use KR_xy =70,000 and KR_z=60,000.
Did you get these these high gains just as the same way you got the value of mass_thrust=132,000 ?

Please,
Could you give some hints about how to adjust the values of Mx, My, Mz in my implementation of LQR onboard controller?

Thanks for all Help.

[whoenig]

For your use-case, you might want to use the new interface that is in SI units, see https://github.com/bitcraze/crazyflie-firmware/blob/master/src/modules/src/controller_brescianini.c#L395-L398 for an example.

Yes, the high gains in Mellinger are because they are not based on SI units. They were found empirically.