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 <[email protected]>

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

Signed-off-by: Silvan <[email protected]>

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

Signed-off-by: Silvan <[email protected]>

---------

Signed-off-by: Silvan <[email protected]>
Co-authored-by: mahima-yoga <[email protected]>

Author: sfuhrer

docs/en/flight_modes_fw/takeoff.md

@@ -85,6 +85,7 @@ The vehicle always respects normal FW max/min throttle settings during takeoff (
 In _catapult/hand-launch mode_ the vehicle waits to detect launch (based on acceleration trigger).
 On launch it enables the motor(s) and climbs with the maximum climb rate [FW_T_CLMB_MAX](#FW_T_CLMB_MAX) while keeping the pitch setpoint above [FW_TKO_PITCH_MIN](#FW_TKO_PITCH_MIN).
 Once it reaches [MIS_TAKEOFF_ALT](#MIS_TAKEOFF_ALT) it will automatically switch to [Hold mode](../flight_modes_fw/hold.md) and loiter.
+It is possible to delay the activation of the motors and control surfaces separately, see parameters [FW_LAUN_MOT_DEL](#FW_LAUN_MOT_DEL), [FW_LAUN_CS_LK_DY](#FW_LAUN_CS_LK_DY) and [CA_CS_LAUN_LK](#CA_CS_LAUN_LK). The later is also exposed in the actuator configuration page under the advanced view.
 
 All RC stick movement is ignored during the full takeoff sequence.
 
@@ -104,7 +105,9 @@ The _launch detector_ is affected by the following parameters:
 | <a id="FW_LAUN_DETCN_ON"></a>[FW_LAUN_DETCN_ON](../advanced_config/parameter_reference.md#FW_LAUN_DETCN_ON) | Enable automatic launch detection. If disabled motors spin up on arming already          |
 | <a id="FW_LAUN_AC_THLD"></a>[FW_LAUN_AC_THLD](../advanced_config/parameter_reference.md#FW_LAUN_AC_THLD)    | Acceleration threshold (acceleration in body-forward direction must be above this value) |
 | <a id="FW_LAUN_AC_T"></a>[FW_LAUN_AC_T](../advanced_config/parameter_reference.md#FW_LAUN_AC_T)             | Trigger time (acceleration must be above threshold for this amount of seconds)           |
-| <a id="FW_LAUN_MOT_DEL"></a>[FW_LAUN_MOT_DEL](../advanced_config/parameter_reference.md#FW_LAUN_MOT_DEL)    | Delay from launch detection to motor spin up                                             |
+| <a id="FW_LAUN_MOT_DEL"></a>[FW_LAUN_MOT_DEL](../advanced_config/parameter_reference.md#FW_LAUN_MOT_DEL)    | Delay from launch detection to motor spin up
+| <a id="FW_LAUN_CS_LK_DY"></a>[FW_LAUN_CS_LK_DY](../advanced_config/parameter_reference.md#FW_LAUN_CS_LK_DY)    | Delay from launch detection to unlocking the control surfaces
+| <a id="CA_CS_LAUN_LK"></a>[CA_CS_LAUN_LK](../advanced_config/parameter_reference.md#CA_CS_LAUN_LK)    | Bitmask to select which control surfaces are to be locked during launch                                             |
 
 ## Runway Takeoff {#runway_launch}
 

msg/LaunchDetectionStatus.msg

@@ -7,3 +7,5 @@ uint8 STATE_LAUNCH_DETECTED_DISABLED_MOTOR 	= 1 # launch detected, but keep moto
 uint8 STATE_FLYING 				= 2 # launch detected, use normal takeoff/flying configuration
 
 uint8 launch_detection_state
+
+bool selected_control_surface_disarmed		# [-] flag indicating whether selected actuators should kept disarmed (have to be configured in control allocation)

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 @@ class ActuatorEffectivenessControlSurfaces : public ModuleParams, public Actuato
 
 	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 @@ class ActuatorEffectivenessControlSurfaces : public ModuleParams, public Actuato
 	};
 	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 @@ class ActuatorEffectivenessFixedWing : public ModuleParams, public ActuatorEffec
 
 	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'

src/modules/fw_mode_manager/FixedWingModeManager.cpp

@@ -1207,6 +1207,7 @@ FixedWingModeManager::control_auto_takeoff(const hrt_abstime &now, const float c
 			_takeoff_init_position = global_position;
 			_takeoff_ground_alt = _current_altitude;
 			_launch_current_yaw = _yaw;
+			_time_launch_detected = now;
 		}
 
 		const Vector2f launch_local_position = _global_local_proj_ref.project(_takeoff_init_position(0),
@@ -1282,6 +1283,9 @@ FixedWingModeManager::control_auto_takeoff(const hrt_abstime &now, const float c
 		launch_detection_status_s launch_detection_status;
 		launch_detection_status.timestamp = now;
 		launch_detection_status.launch_detection_state = _launchDetector.getLaunchDetected();
+		launch_detection_status.selected_control_surface_disarmed =
+			hrt_elapsed_time(&_time_launch_detected) < _param_fw_laun_cs_lk_dy.get() * 1_s
+			|| _time_launch_detected == 0;
 		_launch_detection_status_pub.publish(launch_detection_status);
 	}
 
@@ -1377,6 +1381,7 @@ FixedWingModeManager::control_auto_takeoff_no_nav(const hrt_abstime &now, const
 		if (!_launch_detected && _launchDetector.getLaunchDetected() > launch_detection_status_s::STATE_WAITING_FOR_LAUNCH) {
 			_launch_detected = true;
 			_takeoff_ground_alt = _current_altitude;
+			_time_launch_detected = now;
 		}
 
 		/* Launch has been detected, hence we have to control the plane. */
@@ -1408,6 +1413,9 @@ FixedWingModeManager::control_auto_takeoff_no_nav(const hrt_abstime &now, const
 		launch_detection_status_s launch_detection_status;
 		launch_detection_status.timestamp = now;
 		launch_detection_status.launch_detection_state = _launchDetector.getLaunchDetected();
+		launch_detection_status.selected_control_surface_disarmed =
+			hrt_elapsed_time(&_time_launch_detected) < _param_fw_laun_cs_lk_dy.get() * 1_s
+			|| _time_launch_detected == 0;
 		_launch_detection_status_pub.publish(launch_detection_status);
 	}
 
@@ -2312,6 +2320,8 @@ FixedWingModeManager::reset_takeoff_state()
 
 	_launch_detected = false;
 
+	_time_launch_detected = 0;
+
 	_takeoff_ground_alt = _current_altitude;
 }
 

src/modules/fw_mode_manager/FixedWingModeManager.hpp

@@ -298,6 +298,9 @@ class FixedWingModeManager final : public ModuleBase<FixedWingModeManager>, publ
 	// true if a launch, specifically using the launch detector, has been detected
 	bool _launch_detected{false};
 
+	// [us] time stamp of (runway/catapult) launch detection
+	hrt_abstime _time_launch_detected{0};
+
 	// [deg] global position of the vehicle at the time launch is detected (using launch detector) or takeoff is started (runway)
 	Vector2d _takeoff_init_position{0, 0};
 
@@ -865,6 +868,10 @@ class FixedWingModeManager final : public ModuleBase<FixedWingModeManager>, publ
 		(ParamFloat<px4::params::FW_GPSF_R>) _param_nav_gpsf_r,
 		(ParamFloat<px4::params::FW_T_SPDWEIGHT>) _param_t_spdweight,
 
+		// Launch detection parameters
+		(ParamBool<px4::params::FW_LAUN_DETCN_ON>) _param_fw_laun_detcn_on,
+		(ParamFloat<px4::params::FW_LAUN_CS_LK_DY>) _param_fw_laun_cs_lk_dy,
+
 		// external parameters
 		(ParamBool<px4::params::FW_USE_AIRSPD>) _param_fw_use_airspd,
 		(ParamFloat<px4::params::NAV_LOITER_RAD>) _param_nav_loiter_rad,
@@ -881,7 +888,6 @@ class FixedWingModeManager final : public ModuleBase<FixedWingModeManager>, publ
 		(ParamInt<px4::params::FW_LND_ABORT>) _param_fw_lnd_abort,
 		(ParamFloat<px4::params::FW_TKO_AIRSPD>) _param_fw_tko_airspd,
 		(ParamFloat<px4::params::RWTO_PSP>) _param_rwto_psp,
-		(ParamBool<px4::params::FW_LAUN_DETCN_ON>) _param_fw_laun_detcn_on,
 		(ParamFloat<px4::params::FW_AIRSPD_MAX>) _param_fw_airspd_max,
 		(ParamFloat<px4::params::FW_AIRSPD_MIN>) _param_fw_airspd_min,
 		(ParamFloat<px4::params::FW_AIRSPD_TRIM>) _param_fw_airspd_trim,

src/modules/fw_mode_manager/fw_mode_manager_params.c

@@ -580,6 +580,21 @@ PARAM_DEFINE_INT32(FW_LND_ABORT, 3);
  */
 PARAM_DEFINE_INT32(FW_LAUN_DETCN_ON, 0);
 
+/**
+ * Control surface launch delay
+ *
+ * Locks control surfaces during pre-launch (armed) and until this time since launch has passed.
+ * Only affects control surfaces that have corresponding flag set, and not active for runway takeoff.
+ * Set to 0 to disable any surface locking after arming.
+ *
+ * @unit s
+ * @min 0.0
+ * @decimal 1
+ * @increment 0.1
+ * @group FW Auto Takeoff
+ */
+PARAM_DEFINE_FLOAT(FW_LAUN_CS_LK_DY, 0.f);
+
 /**
  * Flaps setting during take-off
  *