fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

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

* docs: update takeoff docs

---------

Co-authored-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Mahima Yoga 2026-03-24 15:14:42 +00:00 committed by GitHub
parent d617971b3e
commit 5d26d7126a
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
6 changed files with 17 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
docs/en/flight_modes_fw/takeoff.md
View File

@ -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 |

10
src/modules/fw_mode_manager/FixedWingModeManager.cpp
View File

@ -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 {

2
src/modules/fw_mode_manager/FixedWingModeManager.hpp
View File

@ -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{};

6
src/modules/fw_mode_manager/launchdetection/LaunchDetector.cpp
View File

@ -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()) {

6
src/modules/fw_mode_manager/launchdetection/LaunchDetector.h
View File

@ -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

8
src/modules/fw_mode_manager/launchdetection/launchdetection_params.yaml
View File

@ -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