CA: add fixed-wing airmode

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp

@@ -227,6 +227,7 @@ public:
 	int numConfiguredActuators() const { return _num_actuators; }
 
 	void setNormalizeAsPlanarMC(bool normalize_as_mc) { _normalize_matrix_as_planar_mc = normalize_as_mc; }
+	void setAirmode(const int airmode) {_airmode = airmode; }
 
 protected:
 	friend class ControlAllocator; // for _actuator_sp
@@ -244,4 +245,5 @@ protected:
 	int _num_actuators{0};
 	bool _normalize_matrix_as_planar_mc{false};		///< if true, normalize roll, pitch and yaw columns optimized for planar MC
 	bool _had_actuator_failure{false};
+	int _airmode{0};					///< 0: disabled, 1: RP airmode, 2: RPY airmode
 };

src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp

@@ -49,7 +49,7 @@ ControlAllocationSequentialDesaturation::allocate()
 
 	_prev_actuator_sp = _actuator_sp;
 
-	switch (_param_mc_airmode.get()) {
+	switch (_airmode) {
 	case 1:
 		mixAirmodeRP();
 		break;

src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp

@@ -121,8 +121,4 @@ private:
 	 * but yaw is decreased as much as required.
 	 */
 	void mixYaw();
-
-	DEFINE_PARAMETERS(
-		(ParamInt<px4::params::MC_AIRMODE>) _param_mc_airmode   ///< air-mode
-	);
 };

src/modules/control_allocator/ControlAllocator.cpp

@@ -428,6 +428,9 @@ ControlAllocator::Run()
 		c[0](4) = _thrust_sp(1);
 		c[0](5) = _thrust_sp(2);
 
+		const int airmode = _flight_phase != ActuatorEffectiveness::FlightPhase::FORWARD_FLIGHT ? _param_mc_airmode.get() :
+				    _param_ca_fw_dthr_airmd.get();
+
 		if (_num_control_allocation > 1) {
 			matrix::Vector<float, NUM_AXES> vehicle_torque_setpoint_matrix_1;
 
@@ -475,6 +478,7 @@ ControlAllocator::Run()
 
 		for (int i = 0; i < _num_control_allocation; ++i) {
 
+			_control_allocation[i]->setAirmode(airmode);
 			_control_allocation[i]->setControlSetpoint(c[i]);
 
 			// Do allocation

src/modules/control_allocator/ControlAllocator.hpp

@@ -223,7 +223,9 @@ private:
 		(ParamFloat<px4::params::CA_FW_DTHR_SC_Y>) _param_ca_fw_dthr_sc_y,
 		(ParamFloat<px4::params::CA_FW_DTHR_WGT_R>) _param_ca_fw_dthr_wgt_r,
 		(ParamFloat<px4::params::CA_FW_DTHR_WGT_P>) _param_ca_fw_dthr_wgt_p,
-		(ParamFloat<px4::params::CA_FW_DTHR_WGT_Y>) _param_ca_fw_dthr_wgt_y
+		(ParamFloat<px4::params::CA_FW_DTHR_WGT_Y>) _param_ca_fw_dthr_wgt_y,
+		(ParamInt<px4::params::MC_AIRMODE>) _param_mc_airmode,
+		(ParamInt<px4::params::CA_FW_DTHR_AIRMD>) _param_ca_fw_dthr_airmd
 	)
 
 };

src/modules/control_allocator/module.yaml

@@ -606,6 +606,19 @@ parameters:
             min: 0
             max: 1
             default: 0
+
+        CA_FW_DTHR_AIRMD:
+            description:
+                short: Enable airmode for fixed-wing differential thrust
+                long: |
+                    If enabeld it allows the allocator to increase collective thrust to achieve
+                    thorque setpoints.
+            type: enum
+            values:
+                0: Disabled
+                1: Roll/Pitch
+                2: Roll/Pitch/Yaw
+            default: 0
 # Mixer
 mixer:
     actuator_types: