* attitude and rate setpoint message: use 3D array for thrust demand
* FixedWingAttitudeControl: rework airspeed scaling
* move airspeed and scaling calculation into separate method
* if vtol in hover and airspeed disabled use minimum airspeed instead of trim airspeed
When switching back from rate to attitude control, the code depended on a
vehicle_control_mode topics update, but the publication frequency of that
is low. So the switch was noticeably delayed.
- better in terms of dependencies:
- the position controller code depended on position topics for yaw
- mc_pos_control does not have to be run for Stabilized mode
- the code path is much simpler, and thus less error prone. This is
especially important since Stabilized is often used as a fail-safe flight
mode.
The rate controller is now run directly after a gyro publication, and
as soon as it publishes the actuator controls, the output driver (fmu/...)
runs.
Test on a Pixracer:
Reduces fmu control latency from 219us to 134us.
If we run the rate controller last (same order as before, just increase
the prio), the latency is 201us.
CPU load is unchanged.
The drawback is that the attitude to rate setpoint generation is delayed
by one cycle (4ms), but it will be reduced to 1ms as soon as we run at
1kHz.
It scales the yawspeed setpoint arbitrarily by default with 0.5.
This makes no sense because when you give a setpoint of 1rad/s then
you expect the setpoint to get executed. If you want manual yawspeed
response to be less agressive on the stick use the scaling parameter
for the stick MPC_MAN_Y_MAX.
Previously the rate controller disabled updating the integral below 20%
throttle. This is not ideal for several reasons:
- some racers already hover with 20% throttle.
- for acro it is important to always keep the integral enabled, it has a
noticeable effect on flight performance.
The meaning of the yaw weight changed with #8003:
- before, the yaw weight decreased with increasing tilt angle error, so
it was mostly 1
- now, it is constant and depends on the tuning gains (around 0.4 by default)
It means that #8003 reduced the feedforward term, and we get the closer
behavior as before with this change.
It also reduces coupling between different parameters.
- 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
tested on at least 5 different vehicles, including AeroFC. The values
should be conservative, good setups (with low vibrations) can increase
these values even further.
increasing IMU_GYRO_CUTOFF allows for better tuning gains (increased P).
Roman Bapst