feat(fw-launch-detection): use acceleration magnitude instead of body-forward (#26857)

* feat(fw-launch-detection): use acceleration magnitude for launch detection

* docs: update takeoff docs

---------

Co-authored-by: Silvan Fuhrer <silvan@auterion.com>

docs/en/flight_modes_fw/takeoff.md

@@ -103,7 +103,7 @@ The _launch detector_ is affected by the following parameters:
 | Parameter                                                                                                   | Description                                                                              |
 | ----------------------------------------------------------------------------------------------------------- | ---------------------------------------------------------------------------------------- |
 | <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_THLD"></a>[FW_LAUN_AC_THLD](../advanced_config/parameter_reference.md#FW_LAUN_AC_THLD)    | Acceleration threshold (norm of acceleration 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_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                            |

src/modules/fw_mode_manager/FixedWingModeManager.cpp

@@ -253,7 +253,7 @@ FixedWingModeManager::vehicle_attitude_poll()
 		_pitch = euler_angles(1);
 
 		const Vector3f body_acceleration = R.transpose() * Vector3f{_local_pos.ax, _local_pos.ay, _local_pos.az};
-		_body_acceleration_x = body_acceleration(0);
+		_body_acceleration_norm = body_acceleration.norm();
 
 		const Vector3f body_velocity = R.transpose() * Vector3f{_local_pos.vx, _local_pos.vy, _local_pos.vz};
 		_body_velocity_x = body_velocity(0);
@@ -1196,8 +1196,8 @@ FixedWingModeManager::control_auto_takeoff(const hrt_abstime &now, const float c
 			if (_control_mode.flag_armed) {
 				/* Perform launch detection */
 
-				/* Detect launch using body X (forward) acceleration */
+				/* Detect launch using body acceleration norm */
-				_launchDetector.update(control_interval, _body_acceleration_x);
+				_launchDetector.update(control_interval, _body_acceleration_norm);
 			}
 
 		} else	{
@@ -1372,8 +1372,8 @@ FixedWingModeManager::control_auto_takeoff_no_nav(const hrt_abstime &now, const
 		    _launchDetector.getLaunchDetected() < launch_detection_status_s::STATE_FLYING) {
 
 			if (_control_mode.flag_armed) {
-				/* Detect launch using body X (forward) acceleration */
+				/* Detect launch using body acceleration norm */
-				_launchDetector.update(control_interval, _body_acceleration_x);
+				_launchDetector.update(control_interval, _body_acceleration_norm);
 			}
 
 		} else	{

src/modules/fw_mode_manager/FixedWingModeManager.hpp

@@ -258,7 +258,7 @@ private:
 	float _pitch{0.0f}; // [rad] current pitch angle from attitude
 	float _throttle{0.0f}; // [0-1] last set throttle
 
-	float _body_acceleration_x{0.f};
+	float _body_acceleration_norm{0.f};
 	float _body_velocity_x{0.f};
 
 	MapProjection _global_local_proj_ref{};

src/modules/fw_mode_manager/launchdetection/LaunchDetector.cpp

@@ -45,7 +45,7 @@
 namespace launchdetection
 {
 
-void LaunchDetector::update(const float dt, const float accel_x)
+void LaunchDetector::update(const float dt, const float accel_norm)
 {
 	switch (state_) {
 	case launch_detection_status_s::STATE_WAITING_FOR_LAUNCH:
@@ -58,8 +58,8 @@ void LaunchDetector::update(const float dt, const float accel_x)
 			info_delay_counter_s_ = 0.f; // reset counter
 		}
 
-		/* Detect a acceleration that is longer and stronger as the minimum given by the params */
+		/* Detect an acceleration that is longer and stronger than the minimum given by the params */
-		if (accel_x > param_fw_laun_ac_thld_.get()) {
+		if (accel_norm > param_fw_laun_ac_thld_.get()) {
 			acceleration_detected_counter_ += dt;
 
 			if (acceleration_detected_counter_ > param_fw_laun_ac_t_.get()) {

src/modules/fw_mode_manager/launchdetection/LaunchDetector.h

@@ -67,10 +67,10 @@ public:
 	/**
 	 * @brief Updates the state machine based on the current vehicle condition.
 	 *
-	 * @param dt Time step [us]
+	 * @param dt Time step [s]
-	 * @param accel_x Measured acceleration in body x [m/s/s]
+	 * @param accel_norm Norm of acceleration in the body frame [m/s^2]
 	 */
-	void update(const float dt, const float accel_x);
+	void update(const float dt, const float accel_norm);
 
 	/**
 	 * @brief Get the Launch Detected state

src/modules/fw_mode_manager/launchdetection/launchdetection_params.yaml

@@ -5,8 +5,8 @@ parameters:
     FW_LAUN_AC_THLD:
       description:
         short: Trigger acceleration threshold
-        long: Launch is detected when acceleration in body forward direction is above
+        long: Launch is detected when the norm of body acceleration
-          FW_LAUN_AC_THLD for FW_LAUN_AC_T seconds.
+          is above FW_LAUN_AC_THLD for FW_LAUN_AC_T seconds.
       type: float
       default: 30.0
       unit: m/s^2
@@ -16,8 +16,8 @@ parameters:
     FW_LAUN_AC_T:
       description:
         short: Trigger time
-        long: Launch is detected when acceleration in body forward direction is above
+        long: Launch is detected when the norm of body acceleration
-          FW_LAUN_AC_THLD for FW_LAUN_AC_T seconds.
+          is above FW_LAUN_AC_THLD for FW_LAUN_AC_T seconds.
       type: float
       default: 0.05
       unit: s