Hi Pattius,
The Px4 controller documentation points to this report as the basis for the attitude controller, but I’m not sure that covers your question very well.
I agree with your assessment of the function’s purpose. The _accelerationControl()
function is responsible for converting the desired acceleration vector to the desired normalised thrust vector. This is conducted in a world aligned Front-Right-Down (FRD) xyz coordinate frame, which is standard for aerospace systems, and might explain some of the confusion.
It also limits the normalised thrust setpoint to account for the maximum tilt the drone can safely reach while remaining airborne. I think this is might be why the XY components are initially handled seperately to the Z setpoint, rather than combining them as you suggest.
Looking at the code, I would agree that the scaling factor of the _thr_sp
x-y components will be slightly off versus the desired acceleration. Use of the modified collective_thrust
to scale these components introduces effects due to the - hover_thrust
term and its scaling to prioritise the _thr_sp
z-component. The tilit limits will also have an effect, though this one is desired.
That said, I’d expect that the _thr_sp
x-y components will still be roughly proportional to the desired value and point in the correct direction, allowing the larger controller structure to correct for the difference at the position and velocity control levels, so I don’t know how much of a difference this makes to flight performance.
It might make sense to look into the ControlMath::limitTilt()
function and a simulation to compare your suggested approach with what already exists.
Hope this helps,
Robert.
Code Explanation
As far as the maths of this function goes, my understanding of each of the line of the function is:
// Assume standard acceleration due to gravity in vertical direction for attitude generation
Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), CONSTANTS_ONE_G).normalized();
This line computes an initial value for the unit vector orientation of the drone’s body frame z axis, expressed in the world aligned FRD frame {W}
. The drone thrust is going to act in the negative body z-axis so, to provide the desired acceleration in the {W}
xy-axes, the body z-axis must point in the opposite direction to the acceleration setpoint. To hover, the drone will also be providing thust upwards to give acceleration of negative CONSTANTS_ONE_G
in the {W}
z-axis, so the body z-axis must again have this component in the opposite direction.
Normalising this converts it to a unit vector expression of orientation. The rest of the code then caluclates a scaling factor to give the thrust in normalised units.
ControlMath::limitTilt(body_z, Vector3f(0, 0, 1), _constraints.tilt);
This function alters the x-y components of the unit body_z vector to ensure the drone does not exceed the required tilt angle. It modifies body_z
in place, returning a re-normalised unit vector that obeys the tilt limits.
// Scale thrust assuming hover thrust produces standard gravity
float collective_thrust = _acc_sp(2) * (_hover_thrust / CONSTANTS_ONE_G) - _hover_thrust;
This line gives the required normalised thrust setpoint in the {W}
z-axis. Again, the result will likely be negative due to the FRD frame.
The first term gives the thrust required to meet the z-axis acceleration setpoint. Essentially the (_hover_thrust / CONSTANTS_ONE_G)
is a unit conversion factor. I’d expect its units to be something like norm_thrusts/(m/s^2)
, calculated with _horver_thrust
being in the range [0,1] norm_thrusts
and CONSTANTS_ONE_G
being 9.81 m/s^2
.
The second term adds - _hover_thrust
to account for thrust required to hover with drone weight acting in the positive {W}
z-axis.
// Project thrust to planned body attitude
collective_thrust /= (Vector3f(0, 0, 1).dot(body_z));
This divides the collective thrust by the component of the body_z
vector acting in the {W}
z-axis. This scaling step means that the _thr_sp
z-componet will have the magnitude of collective_thrust
calculated in the line above when the two are multiplied together later.
collective_thrust = math::min(collective_thrust, -_lim_thr_min);
This limits the thrust scale to the minimum output that can be produced without stopping the propellers or spinning them in reverse. It prevents excessive downwards acceleration.
_thr_sp = body_z * collective_thrust;
The magnitude of the desired, re-scaled collective_thrust
is applied to the body_z
orientation to give the final thrust setpoint. The negative value of collective_thrust
also flips the body_z
direction giving the correct thrust in the {W}
FRD coordinate frame.