From 5d26d7126aa54741baad66fbd00b2a68c64bc94c Mon Sep 17 00:00:00 2001 From: Mahima Yoga Date: Tue, 24 Mar 2026 15:14:42 +0000 Subject: [PATCH] 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 --- docs/en/flight_modes_fw/takeoff.md | 2 +- src/modules/fw_mode_manager/FixedWingModeManager.cpp | 10 +++++----- src/modules/fw_mode_manager/FixedWingModeManager.hpp | 2 +- .../fw_mode_manager/launchdetection/LaunchDetector.cpp | 6 +++--- .../fw_mode_manager/launchdetection/LaunchDetector.h | 6 +++--- .../launchdetection/launchdetection_params.yaml | 8 ++++---- 6 files changed, 17 insertions(+), 17 deletions(-) diff --git a/docs/en/flight_modes_fw/takeoff.md b/docs/en/flight_modes_fw/takeoff.md index 9f837eda0e..8fd120a27d 100644 --- a/docs/en/flight_modes_fw/takeoff.md +++ b/docs/en/flight_modes_fw/takeoff.md @@ -103,7 +103,7 @@ The _launch detector_ is affected by the following parameters: | Parameter | Description | | ----------------------------------------------------------------------------------------------------------- | ---------------------------------------------------------------------------------------- | | [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 | -| [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) | +| [FW_LAUN_AC_THLD](../advanced_config/parameter_reference.md#FW_LAUN_AC_THLD) | Acceleration threshold (norm of acceleration must be above this value) | | [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) | | [FW_LAUN_MOT_DEL](../advanced_config/parameter_reference.md#FW_LAUN_MOT_DEL) | Delay from launch detection to motor spin up | | [FW_LAUN_CS_LK_DY](../advanced_config/parameter_reference.md#FW_LAUN_CS_LK_DY) | Delay from launch detection to unlocking the control surfaces | diff --git a/src/modules/fw_mode_manager/FixedWingModeManager.cpp b/src/modules/fw_mode_manager/FixedWingModeManager.cpp index 509a4dd8c2..5202e5a913 100644 --- a/src/modules/fw_mode_manager/FixedWingModeManager.cpp +++ b/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 */ - _launchDetector.update(control_interval, _body_acceleration_x); + /* Detect launch using body acceleration norm */ + _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 */ - _launchDetector.update(control_interval, _body_acceleration_x); + /* Detect launch using body acceleration norm */ + _launchDetector.update(control_interval, _body_acceleration_norm); } } else { diff --git a/src/modules/fw_mode_manager/FixedWingModeManager.hpp b/src/modules/fw_mode_manager/FixedWingModeManager.hpp index 00d8625d42..9c3919a456 100644 --- a/src/modules/fw_mode_manager/FixedWingModeManager.hpp +++ b/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{}; diff --git a/src/modules/fw_mode_manager/launchdetection/LaunchDetector.cpp b/src/modules/fw_mode_manager/launchdetection/LaunchDetector.cpp index d463fc75e8..88b5ce5ed0 100644 --- a/src/modules/fw_mode_manager/launchdetection/LaunchDetector.cpp +++ b/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 */ - if (accel_x > param_fw_laun_ac_thld_.get()) { + /* Detect an acceleration that is longer and stronger than the minimum given by the params */ + if (accel_norm > param_fw_laun_ac_thld_.get()) { acceleration_detected_counter_ += dt; if (acceleration_detected_counter_ > param_fw_laun_ac_t_.get()) { diff --git a/src/modules/fw_mode_manager/launchdetection/LaunchDetector.h b/src/modules/fw_mode_manager/launchdetection/LaunchDetector.h index 698b56a0f1..5ed19c1b59 100644 --- a/src/modules/fw_mode_manager/launchdetection/LaunchDetector.h +++ b/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 accel_x Measured acceleration in body x [m/s/s] + * @param dt Time step [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 diff --git a/src/modules/fw_mode_manager/launchdetection/launchdetection_params.yaml b/src/modules/fw_mode_manager/launchdetection/launchdetection_params.yaml index a9573f2202..03c4d3b005 100644 --- a/src/modules/fw_mode_manager/launchdetection/launchdetection_params.yaml +++ b/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 - FW_LAUN_AC_THLD for FW_LAUN_AC_T seconds. + long: Launch is detected when the norm of body acceleration + 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 - FW_LAUN_AC_THLD for FW_LAUN_AC_T seconds. + long: Launch is detected when the norm of body acceleration + is above FW_LAUN_AC_THLD for FW_LAUN_AC_T seconds. type: float default: 0.05 unit: s