From d27ac8a2cbedbca17ad853a080bdd273ed6e8fea Mon Sep 17 00:00:00 2001
From: Silvan Fuhrer <silvan@auterion.com>
Date: Thu, 6 Jun 2024 14:52:20 +0200
Subject: [PATCH] CA: add fixed-wing airmode

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
---
 .../ControlAllocation/ControlAllocation.hpp         |  2 ++
 .../ControlAllocationSequentialDesaturation.cpp     |  2 +-
 .../ControlAllocationSequentialDesaturation.hpp     |  4 ----
 src/modules/control_allocator/ControlAllocator.cpp  |  4 ++++
 src/modules/control_allocator/ControlAllocator.hpp  |  4 +++-
 src/modules/control_allocator/module.yaml           | 13 +++++++++++++
 6 files changed, 23 insertions(+), 6 deletions(-)

diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp
index bba12a30b8..76bb1c9677 100644
--- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp
+++ b/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
 };
diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp
index 00070191c3..6ffb0b972c 100644
--- a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp
+++ b/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;
diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp
index 53c422cd39..e51cb13546 100644
--- a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp
+++ b/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
-	);
 };
diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp
index 8881dcb33d..84423a742b 100644
--- a/src/modules/control_allocator/ControlAllocator.cpp
+++ b/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
diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp
index 4c7dd39426..c5cb8bfeb9 100644
--- a/src/modules/control_allocator/ControlAllocator.hpp
+++ b/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
 	)
 
 };
diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml
index 480215baf7..3c50047909 100644
--- a/src/modules/control_allocator/module.yaml
+++ b/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: