Control Allocation Matrix and Thrust Output

Hi everyone,

I’m wondering how the control allocation matrix below is used to get the thrust output of each motor in the expression below.
Also, what is “_rotors[i].roll_scale” in the thrust output expression? I cannot find the definition of the term.

Control Allocation Matrix for hexacopter X in mixer_multirotor.py script:
P3 = np.matrix([
[-1., 0., -1., 1. ],
[ 1., -0., 1., 1. ],
[ 0.5, 0.87, -1., 1. ],
[-0.5, -0.87, 1., 1. ],
[-0.5, 0.87, 1., 1. ],
[ 0.5, -0.87, -1., 1. ]])

Thrust output of each motor in MultirotorMixer.cpp script:
outputs[i] = roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale + yaw * _rotors[i].yaw_scale + thrust * _rotors[i].thrust_scale;

The matrix elements are copied into the _rotors struct:

  • _rotors[i] is the i-th line of the matrix
  • roll_scale is the first columns, pitch_scale the second, yaw_scale the third, and thrust_scale the fourth.

Thank you very much for your response.
Is my understanding below correct?
_rotors[1] = [-1., 0., -1., 1. ]
_rotors[2] = [ 1., -0., 1., 1. ]
_rotors[3] = [0.5, 0.87, -1., 1. ]
_rotors[4] = [-0.5, -0.87, 1., 1. ]
_rotors[5] = [-0.5, 0.87, 1., 1. ]
_rotors[6] = [ 0.5, -0.87, -1., 1. ]

_rotors[1].roll_scale = -1
_rotors[1].pitch_scale = 0
_rotors[1].yaw_scale = -1
_rotors[1].thrust_scale = 1

_rotors[2].roll_scale = 1
_rotors[2].pitch_scale = -0
_rotors[2].yaw_scale = 1
_rotors[2].thrust_scale = 1
・・・

Where in a script could I find the above relationships?
If you can provide the line# in a script, that would be very helpful.

Yes you are correct, see this line:

Thank you very much for your verification.
I’m sorry for my late reply. It took me time to figure out the script.

I would like to clarify my understanding below and have some questions.

Clarification of my understanding:

So, the thrust output of each motor in MultirotorMixer.cpp script is below.
outputs[i] = roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale + yaw * _rotors[i].yaw_scale + thrust * _rotors[i].thrust_scale;

The above expression can be written as below for hexacopter X.

output[1] = roll * rotors[1].roll_scale + pitch * rotors[1].pitch_scale + yaw * rotors[1].yaw_scale + thrust * rotors[1].thrust_scale
output[2] = roll * rotors[2].roll_scale + pitch * rotors[2].pitch_scale + yaw * rotors[2].yaw_scale + thrust * rotors[2].thrust_scale
output[3] = roll * rotors[3].roll_scale + pitch * rotors[3].pitch_scale + yaw * rotors[3].yaw_scale + thrust * rotors[3].thrust_scale
output[4] = roll * rotors[4].roll_scale + pitch * rotors[4].pitch_scale + yaw * rotors[4].yaw_scale + thrust * rotors[4].thrust_scale
output[5] = roll * rotors[5].roll_scale + pitch * rotors[5].pitch_scale + yaw * rotors[5].yaw_scale + thrust * rotors[5].thrust_scale
output[6] = roll * rotors[6].roll_scale + pitch * rotors[6].pitch_scale + yaw * rotors[6].yaw_scale + thrust * rotors[6].thrust_scale

output[1] = roll * (-1) + pitch * (0) + yaw * (-1) + thrust * (1)
output[2] = roll * (1) + pitch * (0) + yaw * (1) + thrust * (1)
output[3] = roll * (0.5) + pitch * (0.87) + yaw * (-1)+ thrust * (1)
output[4] = roll * (-0.5) + pitch * (-0.87)+ yaw * (1) + thrust * (1)
output[5] = roll * (-0.5)+ pitch * (0.87) + yaw * (1) + thrust * (1)
output[6] = roll * (0.5) + pitch * (-0.87) + yaw * (-1) + thrust * (1)

output[1] = Control Value_roll * _roll_scale * (-1) + Control Value_pitch * _pitch_scale * (0) + Control Value_yaw * _yaw_scale * (-1) + Control Value_thrust * (1)
output[2] = Control Value_roll * _roll_scale * (1) + Control Value_pitch * _pitch_scale * (0) + Control Value_yaw * _yaw_scale * (1) + Control Value_thrust * (1)
output[3] = Control Value_roll * _roll_scale * (0.5) + Control Value_pitch * _pitch_scale * (0.87) + Control Value_yaw * _yaw_scale * (-1) + Control Value_thrust * (1)
output[4] = Control Value_roll * _roll_scale * (-0.5) + Control Value_pitch * _pitch_scale * (-0.87) + Control Value_yaw * _yaw_scale * (1) + Control Value_thrust * (1)
output[5] = Control Value_roll * _roll_scale * (-0.5) + Control Value_pitch * _pitch_scale * (0.87) + Control Value_yaw * _yaw_scale * (1) + Control Value_thrust * (1)
output[6] = Control Value_roll * _roll_scale * (0.5) + Control Value_pitch * _pitch_scale * (-0.87) + Control Value_yaw * _yaw_scale * (-1) + Control Value_thrust * (1)

where
_roll_scale: Scaling factor applied to roll inputs compared to thrust.
_pitch_scale: Scaling factor applied to pitch inputs compared to thrust.
_yaw_scale: Scaling factor applied to yaw inputs compared to thrust.

Question 1:
Are “_roll_scale”, “_pitch_scale”, and “_yaw_scale” calculated values or input values?
If those are calculated values, where could I find the calculation in a script?
If those are input values, what are the name of the parameters in https://dev.px4.io/v1.9.0/en/advanced/parameter_reference.html?

Question 2:
I think “Control Value_roll”, “Control Value_pitch”, and “Control Value_yaw” are comes from the attitude rate controller.
Is my understanding correct?
Where in a script could I find the connection from the attitude rate controller output to the control values in the above expression?

Your third group of equations is wrong. For an hexacopter, XXXX_scale are the numbers you put in your second group of equations.

  1. Precomputed at compile time. This is done in the script I pointed earlier.
  2. The output of the rate controller is published on topic actuator_controls_0, which is then received by a mixer module.

Thank you very much for your prompt response.
The attached below is the steps of my understanding from the second group of equations to the third group of equations. Could you correct which step is incorrect?

Right, my bad. I was mistaken by the variable names of _XXX_scale and _rotors[i].XXX_scale. The first ones are given as input in the mixer files (the numbers after the R: statement), they are most often equal to 1 and you can ignore them. The second ones are computed in the python script.

Thank you very much for your prompt response and checking.
Is my understanding below correct?

From the third group of equations in my post above,
output[1] = Control Value_roll * _roll_scale * (-1) + Control Value_pitch * _pitch_scale * (0) + Control Value_yaw * _yaw_scale * (-1) + Control Value_thrust * (1)
output[2] = Control Value_roll * _roll_scale * (1) + Control Value_pitch * _pitch_scale * (0) + Control Value_yaw * _yaw_scale * (1) + Control Value_thrust * (1)
output[3] = Control Value_roll * _roll_scale * (0.5) + Control Value_pitch * _pitch_scale * (0.87) + Control Value_yaw * _yaw_scale * (-1) + Control Value_thrust * (1)
output[4] = Control Value_roll * _roll_scale * (-0.5) + Control Value_pitch * _pitch_scale * (-0.87) + Control Value_yaw * _yaw_scale * (1) + Control Value_thrust * (1)
output[5] = Control Value_roll * _roll_scale * (-0.5) + Control Value_pitch * _pitch_scale * (0.87) + Control Value_yaw * _yaw_scale * (1) + Control Value_thrust * (1)
output[6] = Control Value_roll * _roll_scale * (0.5) + Control Value_pitch * _pitch_scale * (-0.87) + Control Value_yaw * _yaw_scale * (-1) + Control Value_thrust * (1)

where
_roll_scale: Scaling factor applied to roll inputs compared to thrust.
_pitch_scale: Scaling factor applied to pitch inputs compared to thrust.
_yaw_scale: Scaling factor applied to yaw inputs compared to thrust.

From “hexa_x.main.mix” (https://github.com/PX4/Firmware/blob/master/ROMFS/px4fmu_common/mixers/hexa_x.main.mix),
R: 6X 10000 10000 10000 0

Then,
_roll_scale = 10000
_pitch_scale = 10000
_yaw_scale = 10000

Therefore,
output[1] = Control Value_roll * 10000 * (-1) + Control Value_pitch * 10000 * (0) + Control Value_yaw * 10000 * (-1) + Control Value_thrust * (1)
output[2] = Control Value_roll * 10000 * (1) + Control Value_pitch * 10000 * (0) + Control Value_yaw * 10000 * (1) + Control Value_thrust * (1)
output[3] = Control Value_roll * 10000 * (0.5) + Control Value_pitch 10000 * (0.87) + Control Value_yaw * 10000 * (-1) + Control Value_thrust * (1)
output[4] = Control Value_roll * 10000 * (-0.5) + Control Value_pitch * 10000 * (-0.87) + Control Value_yaw * 10000 * (1) + Control Value_thrust * (1)
output[5] = Control Value_roll * 10000 * (-0.5) + Control Value_pitch * 10000 * (0.87) + Control Value_yaw * 10000 * (1) + Control Value_thrust * (1)
output[6] = Control Value_roll * 10000 * (0.5) + Control Value_pitch * 10000 * (-0.87) + Control Value_yaw * 10000 * (-1) + Control Value_thrust * (1)

where
Control Value_roll: -1.0 to 1.0 from the rate controller
Control Value_pitchl: -1.0 to 1.0 from the rate controller
Control Value_yaw: -1.0 to 1.0 from the rate controller
Control_Value_thrust: 0.0 to 1.0 from the rate controller

Hello,

If you can review my understanding above, that would be much appreciated.

In the last group of equations in my post above, “Control Value_thrust * (1)“ term is too small compared to the other terms such as “ Control Value_roll * 10000 * (0.5)”.
So, I think something is wrong.
Anyone can review the equations above?

See https://dev.px4.io/v1.9.0/en/concept/mixing.html, 10000 means a scale factor equal to 1.

Thank you very much for your response. I got it. I appreciate your help.