FW auto launch: Add option to lock selected surfaces before/during launch (#25799)

* FWModeManager: add option to set flag for disabling actuators during launch

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

* Allocation: add option to each control surface to be locked for launch

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

* FW rate control: reset integral while control surfaces are locked

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

---------

Signed-off-by: Silvan <silvan@auterion.com>
Co-authored-by: mahima-yoga <mahima@auterion.com>

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp

@@ -63,6 +63,7 @@ ActuatorEffectivenessControlSurfaces::ActuatorEffectivenessControlSurfaces(Modul
 	_spoilers_setpoint_with_slewrate.setSlewRate(kSpoilersSlewRate);
 
 	_count_handle = param_find("CA_SV_CS_COUNT");
+	_param_handle_ca_cs_laun_lk = param_find("CA_CS_LAUN_LK");
 	updateParams();
 }
 
@@ -112,6 +113,8 @@ void ActuatorEffectivenessControlSurfaces::updateParams()
 		}
 	}
 
+	param_get(_param_handle_ca_cs_laun_lk, &_param_ca_cs_laun_lk);
+
 	for (int i = 0; i < _count; i++) {
 		param_get(_param_handles[i].type, (int32_t *)&_params[i].type);
 
@@ -234,3 +237,14 @@ void ActuatorEffectivenessControlSurfaces::applySpoilers(float spoilers_control,
 		actuator_sp(i + first_actuator_idx) += _spoilers_setpoint_with_slewrate.getState() * _params[i].scale_spoiler;
 	}
 }
+
+void ActuatorEffectivenessControlSurfaces::applyLaunchLock(int first_actuator_idx,
+		ActuatorVector &actuator_sp)
+{
+	for (int i = 0; i < _count; ++i) {
+
+		if (_param_ca_cs_laun_lk & (1u << i)) {
+			actuator_sp(i + first_actuator_idx) = NAN;
+		}
+	}
+}

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp

@@ -90,6 +90,7 @@ public:
 
 	void applyFlaps(float flaps_control, int first_actuator_idx, float dt, ActuatorVector &actuator_sp);
 	void applySpoilers(float spoilers_control, int first_actuator_idx, float dt, ActuatorVector &actuator_sp);
+	void applyLaunchLock(int first_actuator_idx, ActuatorVector &actuator_sp);
 
 private:
 	void updateParams() override;
@@ -104,9 +105,11 @@ private:
 	};
 	ParamHandles _param_handles[MAX_COUNT];
 	param_t _count_handle;
+	param_t _param_handle_ca_cs_laun_lk;
 
 	Params _params[MAX_COUNT] {};
 	int32_t _count{0};
+	int32_t _param_ca_cs_laun_lk{0};
 
 	SlewRate<float> _flaps_setpoint_with_slewrate;
 	SlewRate<float> _spoilers_setpoint_with_slewrate;

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp

@@ -65,6 +65,18 @@ void ActuatorEffectivenessFixedWing::updateSetpoint(const matrix::Vector<float,
 		ActuatorVector &actuator_sp, const ActuatorVector &actuator_min, const ActuatorVector &actuator_max)
 {
 	stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp);
+
+	// disable selected control surfaces during launch
+	launch_detection_status_s launch_detection_status;
+
+	if (_launch_detection_status_sub.copy(&launch_detection_status)) {
+		if (launch_detection_status.selected_control_surface_disarmed
+		    && hrt_elapsed_time(&launch_detection_status.timestamp) < 100_ms) {
+
+			_control_surfaces.applyLaunchLock(_first_control_surface_idx, actuator_sp);
+
+		}
+	}
 }
 
 void ActuatorEffectivenessFixedWing::allocateAuxilaryControls(const float dt, int matrix_index,

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp

@@ -38,6 +38,7 @@
 #include "ActuatorEffectivenessControlSurfaces.hpp"
 
 #include <uORB/topics/normalized_unsigned_setpoint.h>
+#include <uORB/topics/launch_detection_status.h>
 
 class ActuatorEffectivenessFixedWing : public ModuleParams, public ActuatorEffectiveness
 {
@@ -60,6 +61,7 @@ private:
 
 	uORB::Subscription _flaps_setpoint_sub{ORB_ID(flaps_setpoint)};
 	uORB::Subscription _spoilers_setpoint_sub{ORB_ID(spoilers_setpoint)};
+	uORB::Subscription _launch_detection_status_sub{ORB_ID(launch_detection_status)};
 
 	int _first_control_surface_idx{0}; ///< applies to matrix 1
 

src/modules/control_allocator/module.yaml

@@ -356,6 +356,22 @@ parameters:
             instance_start: 0
             default: 0
 
+        CA_CS_LAUN_LK:
+            description:
+                short: Control surface launch lock enabled
+                long: If actuator launch lock is enabled, this surface is kept at the disarmed value.
+            type: bitmask
+            bit:
+                0: Control Surface 1
+                1: Control Surface 2
+                2: Control Surface 3
+                3: Control Surface 4
+                4: Control Surface 5
+                5: Control Surface 6
+                6: Control Surface 7
+                7: Control Surface 8
+            default: 0
+
         # Tilts
         CA_SV_TL_COUNT:
             description:
@@ -659,7 +675,7 @@ mixer:
 
     rules:
       - select_identifier: 'servo-type' # restrict torque based on servo type
-        apply_identifiers: ['servo-torque-roll', 'servo-torque-pitch', 'servo-torque-yaw', 'servo-scale-flap', 'servo-scale-spoiler']
+        apply_identifiers: ['servo-torque-roll', 'servo-torque-pitch', 'servo-torque-yaw', 'servo-scale-flap', 'servo-scale-spoiler', 'servo-launch-lock']
         items:
         # Convention: horizontal surfaces: up=positive, vertical: right=positive, mixed: up=positive.
         # By default the scale is set to 1/N, where N is the amount of actuators with an effect on
@@ -858,6 +874,12 @@ mixer:
                             label: 'Spoiler Scale'
                             advanced: true
                             identifier: 'servo-scale-spoiler'
+                          - name: 'CA_CS_LAUN_LK'
+                            label: 'Launch Lock'
+                            advanced: true
+                            identifier: 'servo-launch-lock'
+                            index_offset: 0
+                            show_as: 'bitset'
 
             2: # Standard VTOL
                 title: 'Standard VTOL'