From 798cc4f01c9c0c767235a88488cba9db1e36205a Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 17 Nov 2022 10:56:26 +0100 Subject: [PATCH] LandDetectorMC: enforce that LNDMC_Z_VEL_MAX is larger than MPC_LAND_CRWL/MPC_LAND_SPEED (#20614) * LandDetectorMC: enforce that LNDMC_Z_VEL_MAX * 1.2 is below *MPC_LAND_CRWL/MPC_LAND_SPEED Otherwise the _in_descend flag doesn't get set correctly during the last part of the landing, where the descend speed is at MPC_LAND_CRWL or LAND_SPEED. The _in_descend flag is set it the velocity setpoint is >1.1*LNDMC_Z_VEL_MAX. Signed-off-by: Silvan Fuhrer Co-authored-by: Matthias Grob --- .../land_detector/MulticopterLandDetector.cpp | 15 +++++++++++++++ .../land_detector/MulticopterLandDetector.h | 5 ++++- .../land_detector/land_detector_params_mc.c | 9 +++++---- 3 files changed, 24 insertions(+), 5 deletions(-) diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 8e1f16b8c3..9786a48f46 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -79,6 +79,8 @@ MulticopterLandDetector::MulticopterLandDetector() _paramHandle.minThrottle = param_find("MPC_THR_MIN"); _paramHandle.useHoverThrustEstimate = param_find("MPC_USE_HTE"); _paramHandle.hoverThrottle = param_find("MPC_THR_HOVER"); + _paramHandle.landSpeed = param_find("MPC_LAND_SPEED"); + _paramHandle.crawlSpeed = param_find("MPC_LAND_CRWL"); _minimum_thrust_8s_hysteresis.set_hysteresis_time_from(false, 8_s); } @@ -118,6 +120,19 @@ void MulticopterLandDetector::_update_params() { param_get(_paramHandle.minThrottle, &_params.minThrottle); param_get(_paramHandle.minManThrottle, &_params.minManThrottle); + param_get(_paramHandle.landSpeed, &_params.landSpeed); + param_get(_paramHandle.crawlSpeed, &_params.crawlSpeed); + + // 1.2 corresponds to the margin between the default parameters LNDMC_Z_VEL_MAX = MPC_LAND_CRWL / 1.2 + const float lndmc_upper_threshold = math::min(_params.crawlSpeed, _params.landSpeed) / 1.2f; + + if (_param_lndmc_z_vel_max.get() > lndmc_upper_threshold) { + PX4_ERR("LNDMC_Z_VEL_MAX > MPC_LAND_CRWL or MPC_LAND_SPEED, updating %.3f -> %.3f", + (double)_param_lndmc_z_vel_max.get(), (double)(lndmc_upper_threshold)); + + _param_lndmc_z_vel_max.set(lndmc_upper_threshold); + _param_lndmc_z_vel_max.commit_no_notification(); + } int32_t use_hover_thrust_estimate = 0; param_get(_paramHandle.useHoverThrustEstimate, &use_hover_thrust_estimate); diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 98caf04f27..785a6ada3f 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -87,12 +87,13 @@ private: /** Distance above ground below which entering ground contact state is possible when distance to ground is available. */ static constexpr float DIST_FROM_GROUND_THRESHOLD = 1.0f; - /** Handles for interesting parameters. **/ struct { param_t minThrottle; param_t hoverThrottle; param_t minManThrottle; param_t useHoverThrustEstimate; + param_t landSpeed; + param_t crawlSpeed; } _paramHandle{}; struct { @@ -100,6 +101,8 @@ private: float hoverThrottle; float minManThrottle; bool useHoverThrustEstimate; + float landSpeed; + float crawlSpeed; } _params{}; uORB::Subscription _actuator_controls_sub{ORB_ID(actuator_controls_0)}; diff --git a/src/modules/land_detector/land_detector_params_mc.c b/src/modules/land_detector/land_detector_params_mc.c index 889085e79e..b68b1255bf 100644 --- a/src/modules/land_detector/land_detector_params_mc.c +++ b/src/modules/land_detector/land_detector_params_mc.c @@ -51,12 +51,13 @@ PARAM_DEFINE_FLOAT(LNDMC_TRIG_TIME, 1.0f); * Multicopter vertical velocity threshold * * Vertical velocity threshold to detect landing. - * Should be set lower than the expected minimal speed for landing - * so MPC_LAND_SPEED for autonomous landing and MPC_LAND_CRWL - * if distance sensor is present and slowing down close to ground. + * Has to be set lower than the expected minimal speed for landing, + * which is either MPC_LAND_SPEED or MPC_LAND_CRWL. + * This is enforced by an automatic check. * * @unit m/s - * @decimal 1 + * @min 0 + * @decimal 2 * * @group Land Detector */