From ffb8fb438392bcb2c47a452fe4dc65b0831198e2 Mon Sep 17 00:00:00 2001
From: Silvan Fuhrer <silvan@auterion.com>
Date: Mon, 8 Aug 2022 13:28:17 +0200
Subject: [PATCH] Allocation: add SteeringWheel type control surface

Directly use yaw controls for it, and don't add it to the allocation matrix,
as that would have an effect on rudder scaling if the wheel also would have
a yaw effectiveness.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
---
 .../ActuatorEffectivenessControlSurfaces.cpp        | 13 +++++++++++--
 .../ActuatorEffectivenessControlSurfaces.hpp        |  5 +++--
 .../ActuatorEffectivenessFixedWing.cpp              |  8 +++++---
 .../ActuatorEffectivenessStandardVTOL.cpp           |  8 +++++---
 .../ActuatorEffectivenessTiltrotorVTOL.cpp          |  4 +++-
 src/modules/control_allocator/module.yaml           |  5 +++++
 6 files changed, 32 insertions(+), 11 deletions(-)

diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp
index 27288299b6..58b4579e48 100644
--- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp
+++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp
@@ -129,6 +129,11 @@ void ActuatorEffectivenessControlSurfaces::updateParams()
 
 		case Type::SingleChannelAileron:
 			break;
+
+		case Type::SteeringWheel:
+			torque.setZero();
+			break;
+
 		}
 	}
 }
@@ -146,8 +151,8 @@ bool ActuatorEffectivenessControlSurfaces::addActuators(Configuration &configura
 	return true;
 }
 
-void ActuatorEffectivenessControlSurfaces::applyFlapsAndAirbrakes(float flaps_control, float airbrakes_control,
-		int first_actuator_idx,
+void ActuatorEffectivenessControlSurfaces::applyFlapsAirbrakesWheel(float flaps_control, float airbrakes_control,
+		float wheel_control, int first_actuator_idx,
 		ActuatorVector &actuator_sp) const
 {
 	for (int i = 0; i < _count; ++i) {
@@ -165,6 +170,10 @@ void ActuatorEffectivenessControlSurfaces::applyFlapsAndAirbrakes(float flaps_co
 			actuator_sp(i + first_actuator_idx) += airbrakes_control;
 			break;
 
+		case ActuatorEffectivenessControlSurfaces::Type::SteeringWheel:
+			actuator_sp(i + first_actuator_idx) += wheel_control;
+			break;
+
 		default:
 			break;
 		}
diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp
index efb51e0d6e..bb45604ce7 100644
--- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp
+++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp
@@ -60,6 +60,7 @@ public:
 		LeftATail = 13,
 		RightATail = 14,
 		SingleChannelAileron = 15,
+		SteeringWheel = 16,
 	};
 
 	struct Params {
@@ -80,8 +81,8 @@ public:
 
 	const Params &config(int idx) const { return _params[idx]; }
 
-	void applyFlapsAndAirbrakes(float flaps_control, float airbrakes_control, int first_actuator_idx,
-				    ActuatorVector &actuator_sp) const;
+	void applyFlapsAirbrakesWheel(float flaps_control, float airbrakes_control, float wheel_control, int first_actuator_idx,
+				      ActuatorVector &actuator_sp) const;
 
 private:
 	void updateParams() override;
diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp
index bddc95e913..97b7f89c0b 100644
--- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp
+++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp
@@ -68,8 +68,10 @@ void ActuatorEffectivenessFixedWing::updateSetpoint(const matrix::Vector<float,
 	actuator_controls_s actuator_controls_0;
 
 	if (_actuator_controls_0_sub.copy(&actuator_controls_0)) {
-		float control_flaps = actuator_controls_0.control[actuator_controls_s::INDEX_FLAPS];
-		float airbrakes_control = actuator_controls_0.control[actuator_controls_s::INDEX_AIRBRAKES];
-		_control_surfaces.applyFlapsAndAirbrakes(control_flaps, airbrakes_control, _first_control_surface_idx, actuator_sp);
+		const float flaps_control = actuator_controls_0.control[actuator_controls_s::INDEX_FLAPS];
+		const float airbrakes_control = actuator_controls_0.control[actuator_controls_s::INDEX_AIRBRAKES];
+		const float steering_wheel_control = actuator_controls_0.control[actuator_controls_s::INDEX_YAW];
+		_control_surfaces.applyFlapsAirbrakesWheel(flaps_control, airbrakes_control, steering_wheel_control,
+				_first_control_surface_idx, actuator_sp);
 	}
 }
diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp
index 692f76e53c..63fa1481c9 100644
--- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp
+++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp
@@ -71,9 +71,11 @@ void ActuatorEffectivenessStandardVTOL::updateSetpoint(const matrix::Vector<floa
 		actuator_controls_s actuator_controls_1;
 
 		if (_actuator_controls_1_sub.copy(&actuator_controls_1)) {
-			float control_flaps = actuator_controls_1.control[actuator_controls_s::INDEX_FLAPS];
-			float airbrakes_control = actuator_controls_1.control[actuator_controls_s::INDEX_AIRBRAKES];
-			_control_surfaces.applyFlapsAndAirbrakes(control_flaps, airbrakes_control, _first_control_surface_idx, actuator_sp);
+			const float flaps_control = actuator_controls_1.control[actuator_controls_s::INDEX_FLAPS];
+			const float airbrakes_control = actuator_controls_1.control[actuator_controls_s::INDEX_AIRBRAKES];
+			const float steering_wheel_control = actuator_controls_1.control[actuator_controls_s::INDEX_YAW];
+			_control_surfaces.applyFlapsAirbrakesWheel(flaps_control, airbrakes_control, steering_wheel_control,
+					_first_control_surface_idx, actuator_sp);
 		}
 	}
 }
diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
index 675548cab6..b2c14e6c54 100644
--- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
+++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
@@ -101,7 +101,9 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
 		if (_actuator_controls_1_sub.copy(&actuator_controls_1)) {
 			const float flaps_control = actuator_controls_1.control[actuator_controls_s::INDEX_FLAPS];
 			const float airbrakes_control = actuator_controls_1.control[actuator_controls_s::INDEX_AIRBRAKES];
-			_control_surfaces.applyFlapsAndAirbrakes(flaps_control, airbrakes_control, _first_control_surface_idx, actuator_sp);
+			const float steering_wheel_control = actuator_controls_1.control[actuator_controls_s::INDEX_YAW];
+			_control_surfaces.applyFlapsAirbrakesWheel(flaps_control, airbrakes_control, steering_wheel_control,
+					_first_control_surface_idx, actuator_sp);
 		}
 	}
 
diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml
index 1df4ae649c..0159ab1113 100644
--- a/src/modules/control_allocator/module.yaml
+++ b/src/modules/control_allocator/module.yaml
@@ -271,6 +271,7 @@ parameters:
                 13: Left A-tail
                 14: Right A-tail
                 15: Single Channel Aileron
+                16: Steering Wheel
             num_instances: *max_num_servos
             instance_start: 0
             default: 0
@@ -544,6 +545,10 @@ mixer:
               - { 'min': 0.0, 'max': 1.0, 'default': 1.0 } # roll
               - { 'hidden': True, 'default': 0.0 } # pitch
               - { 'hidden': True, 'default': 0.0 } # yaw
+            16: # Steering Wheel
+              - { 'hidden': True, 'default': 0.0 } # roll
+              - { 'hidden': True, 'default': 0.0 } # pitch
+              - { 'hidden': True, 'default': 0.0 } # yaw
 
       - select_identifier: 'servo-type-tailsitter' # restrict torque based on servo type for tailsitters
         apply_identifiers: ['servo-torque-roll-tailsitter', 'servo-torque-pitch-tailsitter', 'servo-torque-yaw-tailsitter']