FW Rate Controller: fix saturation logic for VTOLs with differential thrust enabled

[sfuhrer]

Context: #21501 (comment)

Solved Problem

Actuator saturation logic in the FW rate controller was not working as intended if the actuators for a certain axis aren't in the matrix 1 (servo matrix) for VTOL. That led to the rate controller being very often disabled as the controller thought it the axis was saturated. That in turn leads to very bad rate tracking.
It is most prominent when flying a Quad tailsitter without control surfaces, as in this case all 3 axis are affected.

Solution

Add logic for when to use which allocation matrix.

Changelog Entry

For release notes:

Bugfix: fix actuator saturation handling for VTOLs with differential thrust in fixed-wing flight

Alternatives

We need to work towards keeping this customization per vehicle in the allocation.

Test coverage

SITL tested.

I would port this to 1.14.

[MaEtUgR]

led to the rate controller being very often disabled

You mean integrator part I assume?

We had similar problems on helicopters. The allocation now does some interfacing with the saturation logic, it works but is not ideal:

PX4-Autopilot/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp

Lines 222 to 254 in fc35792

	void ActuatorEffectivenessHelicopter::getUnallocatedControl(int matrix_index, control_allocator_status_s &status)
	{
	// Note: the values '-1', '1' and '0' are just to indicate a negative,
	// positive or no saturation to the rate controller. The actual magnitude is not used.
	if (_saturation_flags.roll_pos) {
	status.unallocated_torque[0] = 1.f;
	} else if (_saturation_flags.roll_neg) {
	status.unallocated_torque[0] = -1.f;
	}
	if (_saturation_flags.pitch_pos) {
	status.unallocated_torque[1] = 1.f;
	} else if (_saturation_flags.pitch_neg) {
	status.unallocated_torque[1] = -1.f;
	}
	if (_saturation_flags.yaw_pos) {
	status.unallocated_torque[2] = 1.f;
	} else if (_saturation_flags.yaw_neg) {
	status.unallocated_torque[2] = -1.f;
	}
	if (_saturation_flags.thrust_pos) {
	status.unallocated_thrust[2] = 1.f;
	} else if (_saturation_flags.thrust_neg) {
	status.unallocated_thrust[2] = -1.f;
	}
	}

towards keeping this customization per vehicle in the allocation

Definitely 💯

[MaEtUgR]

@sfuhrer I reworked it 😇 What do you think? Please check if it really works as expected.

[sfuhrer]

@MaEtUgR thanks a lot for the rework, much cleaner and it now even does what it was intended to do.
I've rebased and squashed the commits, ready to go from my side.

[MaEtUgR]

👍

[sfuhrer]

CI was complaining about _param_vt_fw_difthr_en being int and not int32_t. Addressed and force pushed.