From b4273bde2547cae236889a580697ce789d33dddf Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Wed, 8 Jan 2025 16:56:09 +0100 Subject: [PATCH] FW land detector: Introduce max rotational speed condition (new param: LNDFW_ROT_MAX). Checks that the filtered norm of the angular velocity is below LNDFW_ROT_MAX. --- .../land_detector/FixedwingLandDetector.cpp | 23 ++++++++++++++----- .../land_detector/FixedwingLandDetector.h | 2 ++ .../land_detector/land_detector_params_fw.c | 11 +++++++++ 3 files changed, 30 insertions(+), 6 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index eadfff83a0..4ed4a353ab 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -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. diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index 981fdc2fb5..3f3fa9ad35 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -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) _param_lndfw_airspd, (ParamFloat) _param_lndfw_vel_xy_max, (ParamFloat) _param_lndfw_vel_z_max, + (ParamFloat) _param_lndfw_rot_max, (ParamFloat) _param_lndfw_trig_time ); }; diff --git a/src/modules/land_detector/land_detector_params_fw.c b/src/modules/land_detector/land_detector_params_fw.c index 02ab459032..6ab78fb867 100644 --- a/src/modules/land_detector/land_detector_params_fw.c +++ b/src/modules/land_detector/land_detector_params_fw.c @@ -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);