From 24d871f7929f08a84f40d95e29329cdb60c3133e Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 7 Dec 2021 17:42:03 +0100 Subject: [PATCH] FW land detector: tighten thresholds in airspeed-less case Signed-off-by: Silvan Fuhrer --- .../land_detector/FixedwingLandDetector.cpp | 15 ++++++++++--- .../land_detector/land_detector_params_fw.c | 22 +++++++++++-------- 2 files changed, 25 insertions(+), 12 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 845e6cd058..4c06db04eb 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2021 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -80,9 +80,12 @@ bool FixedwingLandDetector::_get_landed_state() airspeed_validated_s airspeed_validated{}; _airspeed_validated_sub.copy(&airspeed_validated); + bool airspeed_invalid = false; + // set _airspeed_filtered to 0 if airspeed data is invalid if (!PX4_ISFINITE(airspeed_validated.true_airspeed_m_s) || hrt_elapsed_time(&airspeed_validated.timestamp) > 1_s) { _airspeed_filtered = 0.0f; + airspeed_invalid = true; } else { _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * airspeed_validated.true_airspeed_m_s; @@ -93,10 +96,16 @@ 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; + // make thresholds 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_z_max_threshold = airspeed_invalid ? 0.7f * _param_lndfw_vel_z_max.get() : + _param_lndfw_vel_z_max.get(); + // Crude land detector for fixedwing. landDetected = _airspeed_filtered < _param_lndfw_airspd.get() - && _velocity_xy_filtered < _param_lndfw_vel_xy_max.get() - && _velocity_z_filtered < _param_lndfw_vel_z_max.get() + && _velocity_xy_filtered < vel_xy_max_threshold + && _velocity_z_filtered < vel_z_max_threshold && _xy_accel_filtered < _param_lndfw_xyaccel_max.get(); } else { diff --git a/src/modules/land_detector/land_detector_params_fw.c b/src/modules/land_detector/land_detector_params_fw.c index b33c67080c..b2ad0561a0 100644 --- a/src/modules/land_detector/land_detector_params_fw.c +++ b/src/modules/land_detector/land_detector_params_fw.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014-2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2014-2021 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,9 +32,11 @@ ****************************************************************************/ /** - * Fixedwing max horizontal velocity + * Fixed-wing land detector: Max horizontal velocity threshold * - * Maximum horizontal velocity allowed in the landed state + * Maximum horizontal velocity allowed in the landed state. + * A factor of 0.7 is applied in case of airspeed-less flying + * (either because no sensor is present or sensor data got invalid in flight). * * @unit m/s * @min 0.5 @@ -46,9 +48,11 @@ PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 5.0f); /** - * Fixedwing max climb rate + * Fixed-wing land detector: Max vertiacal velocity threshold * - * Maximum vertical velocity allowed in the landed state + * Maximum vertical velocity allowed in the landed state. + * A factor of 0.7 is applied in case of airspeed-less flying + * (either because no sensor is present or sensor data got invalid in flight). * * @unit m/s * @min 0.1 @@ -57,10 +61,10 @@ PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 5.0f); * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LNDFW_VEL_Z_MAX, 3.0f); +PARAM_DEFINE_FLOAT(LNDFW_VEL_Z_MAX, 2.0f); /** - * Fixedwing max horizontal acceleration + * Fixed-wing land detector: Max horizontal acceleration * * Maximum horizontal (x,y body axes) acceleration allowed in the landed state * @@ -74,12 +78,12 @@ PARAM_DEFINE_FLOAT(LNDFW_VEL_Z_MAX, 3.0f); PARAM_DEFINE_FLOAT(LNDFW_XYACC_MAX, 8.0f); /** - * Airspeed max + * Fixed-wing land detector: Max airspeed * * Maximum airspeed allowed in the landed state * * @unit m/s - * @min 4 + * @min 2 * @max 20 * @decimal 1 *