Control Allocation Matrix and Thrust Output

crest_wave · January 15, 2020, 12:56pm

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;

jlecoeur · January 18, 2020, 9:10am

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.

crest_wave · January 21, 2020, 11:59am

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.

jlecoeur · January 21, 2020, 6:03pm

Yes you are correct, see this line:

github.com

PX4/Firmware/blob/master/src/lib/mixer/MultirotorMixer/geometries/tools/px_generate_mixers.py#L265


    buf.write(u"static constexpr MultirotorMixer::Rotor _config_{}[] {{\n".format(geometry['info']['name']))


    for row in mix:
        if use_6dof:
        # 6dof mixer
            buf.write(u"\t{{ {:9f}, {:9f}, {:9f}, {:9f}, {:9f}, {:9f} }},\n".format(
                row[0], row[1], row[2],
                row[3], row[4], row[5]))
        else:
        # 4dof mixer
            buf.write(u"\t{{ {:9f}, {:9f}, {:9f}, {:9f} }},\n".format(
                row[0], row[1], row[2],
                -row[5]))  # Upward thrust is positive TODO: to remove this, adapt PX4 to use NED correctly


    buf.write(u"};\n\n")


# Print geometry indeces
buf.write(u"static constexpr const MultirotorMixer::Rotor *_config_index[] {\n")
for geometry in geometries_list:
    buf.write(u"\t&_config_{}[0],\n".format(geometry['info']['name']))
buf.write(u"};\n\n")

crest_wave · January 30, 2020, 6:15am

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?

jlecoeur · January 30, 2020, 6:44am

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

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

crest_wave · January 30, 2020, 2:04pm

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?

jlecoeur · January 30, 2020, 5:58pm

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.

crest_wave · January 31, 2020, 10:37am

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

crest_wave · February 4, 2020, 12:09am

Hello,

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

crest_wave · February 5, 2020, 11:29pm

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?

jlecoeur · February 11, 2020, 9:55am

See Mixing and Actuators · PX4 Developer Guide, 10000 means a scale factor equal to 1.

crest_wave · February 12, 2020, 3:06pm

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

Daniel_Blanquez · March 24, 2020, 3:53pm

@jlecoeur Hi,

Thanks for you explanation on the subject is really was helpful. I was wondering what are exactly px4 uses the thrust of each motor (output(i)). It represents the thrust of each motor normalized right ? How is this used to control the motor and send motor command like PWM ?