Control Allocation: fixes in yaw saturation detection for vehicles with tilt-for-yaw

[sfuhrer]

Solved Problem

VTOL Tiltrotors require a special yaw saturation detection. Simply looking at whether there is unallocated yaw torque leads to almost constant saturation reporting, as often one of the tilts is saturated (they usually can't tilt backwards). I patched this previously in #20518, where I simply declared the yaw saturated if the torque setpoint was saturated. Given that there is also a collective tilt this assumption is quite rough.

While flying a tricopter with sideways tiltable tail I further noticed that there the reported unallocated yaw was never 0, while the tilt servo clearly never reached saturation:

I didn't investigate further why the unallocated torque calculation is so wrong, but given that MC tilt vehicles would anyway have the same issues as VTOL tiltrotor if they have tilts that only tilt in one direction, it makes sense to me to customize the saturation handling anyway also for them.

Solution

Customize yaw saturation detection for VTOL tiltrotor and MC tilts. Taking the sign of the yaw effectiveness of the corresponding tilt into account, check if the tilt is at the min or max value and set the positive or negative saturation flag if all the tilts are saturated in this direction.

Changelog Entry

For release notes:

Bugfix: fix yaw saturation logic for tilting motors (tiltrotor VTOL and MC with tilts)

Alternatives

Extend it to have it generic for all axis with tilt actuation. I wonder though if we also should first remove the tilts from the effectiveness matrix and do custom allocation of them (something similar to what is done in the Helicopter allocation).

Test coverage

SITL tested (tiltrotor).

[bkueng]

Makes sense.