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

FW land detector: Introduce max rotational speed condition (new param: LNDFW_ROT_MAX).

Browse Source 
Checks that the filtered norm of the angular velocity is below LNDFW_ROT_MAX.
This commit is contained in:
mahimayoga 2025-01-08 16:56:09 +01:00 committed by Silvan Fuhrer
parent 18b4b18a75
commit b4273bde25
3 changed files with 30 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
src/modules/land_detector/FixedwingLandDetector.cpp
View File

@ -109,15 +109,26 @@ bool FixedwingLandDetector::_get_landed_state()
const float acc_hor = matrix::Vector2f(_acceleration).norm();
_xy_accel_filtered = _xy_accel_filtered * 0.8f + acc_hor * 0.18f;
// Check for angular velocity
const float rot_vel_hor = _angular_velocity.norm();
val = _velocity_rot_filtered * 0.95f + rot_vel_hor * 0.05f;
if (PX4_ISFINITE(val)) {
_velocity_rot_filtered = val;
}
// make groundspeed threshold tighter if airspeed is invalid
const float vel_xy_max_threshold = airspeed_invalid ? 0.7f * _param_lndfw_vel_xy_max.get() :
_param_lndfw_vel_xy_max.get();
const float vel_xy_max_threshold = airspeed_invalid ? 0.7f * _param_lndfw_vel_xy_max.get() :
_param_lndfw_vel_xy_max.get();
const float max_rotation_threshold = math::radians(_param_lndfw_rot_max.get()) ;
// Crude land detector for fixedwing.
landDetected = _airspeed_filtered < _param_lndfw_airspd.get()
&& _velocity_xy_filtered < vel_xy_max_threshold
&& _velocity_z_filtered < _param_lndfw_vel_z_max.get()
&& _xy_accel_filtered < _param_lndfw_xyaccel_max.get();
landDetected = _airspeed_filtered < _param_lndfw_airspd.get()
&& _velocity_xy_filtered < vel_xy_max_threshold
&& _velocity_z_filtered < _param_lndfw_vel_z_max.get()
&& _xy_accel_filtered < _param_lndfw_xyaccel_max.get()
&& _velocity_rot_filtered < max_rotation_threshold;
} else {
// Control state topic has timed out and we need to assume we're landed.

2
src/modules/land_detector/FixedwingLandDetector.h
View File

@ -75,6 +75,7 @@ private:
float _velocity_xy_filtered{0.0f};
float _velocity_z_filtered{0.0f};
float _xy_accel_filtered{0.0f};
float _velocity_rot_filtered{0.0f};
DEFINE_PARAMETERS_CUSTOM_PARENT(
LandDetector,
@ -82,6 +83,7 @@ private:
(ParamFloat<px4::params::LNDFW_AIRSPD_MAX>) _param_lndfw_airspd,
(ParamFloat<px4::params::LNDFW_VEL_XY_MAX>) _param_lndfw_vel_xy_max,
(ParamFloat<px4::params::LNDFW_VEL_Z_MAX>) _param_lndfw_vel_z_max,
(ParamFloat<px4::params::LNDFW_ROT_MAX>) _param_lndfw_rot_max,
(ParamFloat<px4::params::LNDFW_TRIG_TIME>) _param_lndfw_trig_time
);
};

11
src/modules/land_detector/land_detector_params_fw.c
View File

@ -102,3 +102,14 @@ PARAM_DEFINE_FLOAT(LNDFW_AIRSPD_MAX, 6.00f);
* @group Land Detector
*/
PARAM_DEFINE_FLOAT(LNDFW_TRIG_TIME, 2.f);
/**
* Fixed-wing land detector: max rotational speed
*
* Maximum allowed norm of the angular velocity in the landed state.
*
* @unit deg/s
* @decimal 1
* @group Land Detector
*/
PARAM_DEFINE_FLOAT(LNDFW_ROT_MAX, 0.5f);