- On initialization _v_att_sp got filled with zeros
leaving invalid quaternions
- While not armed mc_pos_control did not publish any
attitude setpoint which makes no sense
- The attitude control just uses the data in _v_att_sp
if it was (ever) updated or not
While operating on exactly normalized float quaternions
it can aparently still happen that one of the elements
gets just slightly above 1 or below -1 and hence out of
the domain of the acosf and asinf functions resulting in
NaN. The constrain function uses stricly smaller/bigger
comparisons and catches all tested cases.
- Delete left over identity matrix.
- Corner case with a zero element when using the signum function:
We always need a sign also for zero.
- Corner case with arbitrary yaw but and 180 degree roll or pitch:
Reduced attitude control calculation always rotates around roll
because there's no right choice when neglecting yaw. In that small
corner case it's better to just use full attitude contol and hence
rotate around the most efficient roll/pitch combination.
According to the paper the quaternion controller is built on
the yaw weight represents the ratio between the roll/pitch and
the yaw attitude control time constant. It also states that as a
thumb rule a value of ~0.4 works alright for most multicopter
platforms. The default attitude gains of PX4 which were determined
independent of the paper from experimental results have a ratio of
2.8/6.5 = 0.43 which matches.
Because the parameter does not make sense from a control theory
perspective. Either you have a gain with the unit 1/s or an inverse
gain or time constant with the unit s. But the time constant parameter
was neither bound to any exact unit nor did it apply instead of a gain.
Rather it adjusted multiple gains from rate and attitude control
according to an arbitrary scale. This can only by accident lead to
good tuning.
to prioritize yaw compared to roll and pitch by combining
the shortest rotation to achieve a total thrust vector with
the full attitude respecting the desired yaw
not by scaling down the control output with the gains
Previously, the yaw weight was based on the tilt angle of the attitude
setpoint (R_sp(2, 2) == cos(tilt angle)). This makes no sense, it means
the weight is low for high tilt angles even if there is no roll-pitch error
at all.
This patch changes the weight computation to be based on the tilt angle
error: the yaw weight is 1 if there is no roll-pitch error (independent
from current tilt angle), and is reduced for higher tilt angle errors.
The weight was added in 05e9a30573, without any explanation or
derivation of how and why the weight is chosen that way.
However this patch works much better in practice. The yaw control is
improved, though it can be a bit slow to converge if you do continuous &
fast roll-pitch motions (which is expected).
The feedforwarding was applied in the wrong frame: the term is given in
global coordinates, but was directly applied to body coordinates. This
patch adds the missing transformation from global to body frame.
In addition, it moves the feedforwarding before the rate limitation, so
that we cannot exceed the configured rates.
This allows for a quicker update from sensors if the estimator runs at the default priority of the estimator class. If there is no direct sensor pass-through then it will wait for an estimator update.
Daniel Agar