From 27309a45cc9760e62dddb3ff154e9935050ad00e Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 17 Oct 2022 09:47:05 +0200 Subject: [PATCH] MC LandDetector: remove dependency on MPC_LAND_SPEED and MPC_LAND_CRWL Don't consider these params for vertical speed threshold, and instead increase the default for LNDMC_Z_VEL_MAX and use solely that one. Makes the land detector outcome more predictable and its interal logic simpler, while the new default tuning is resulting in about the same vz threshold as before. Signed-off-by: Silvan Fuhrer --- .../land_detector/MulticopterLandDetector.cpp | 25 ++++--------------- .../land_detector/MulticopterLandDetector.h | 6 +---- .../land_detector/land_detector_params_mc.c | 6 +++-- .../mc_pos_control/mc_pos_control_params.c | 5 ++-- 4 files changed, 13 insertions(+), 29 deletions(-) diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index e3c0e23122..c329b684c0 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2022 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 @@ -75,12 +75,10 @@ namespace land_detector MulticopterLandDetector::MulticopterLandDetector() { - _paramHandle.landSpeed = param_find("MPC_LAND_SPEED"); _paramHandle.minManThrottle = param_find("MPC_MANTHR_MIN"); _paramHandle.minThrottle = param_find("MPC_THR_MIN"); _paramHandle.useHoverThrustEstimate = param_find("MPC_USE_HTE"); _paramHandle.hoverThrottle = param_find("MPC_THR_HOVER"); - _paramHandle.crawlSpeed = param_find("MPC_LAND_CRWL"); } void MulticopterLandDetector::_update_topics() @@ -119,16 +117,6 @@ 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); - - if (_param_lndmc_z_vel_max.get() > _params.landSpeed) { - PX4_ERR("LNDMC_Z_VEL_MAX > MPC_LAND_SPEED, updating %.3f -> %.3f", - (double)_param_lndmc_z_vel_max.get(), (double)_params.landSpeed); - - _param_lndmc_z_vel_max.set(_params.landSpeed); - _param_lndmc_z_vel_max.commit_no_notification(); - } int32_t use_hover_thrust_estimate = 0; param_get(_paramHandle.useHoverThrustEstimate, &use_hover_thrust_estimate); @@ -158,22 +146,19 @@ bool MulticopterLandDetector::_get_ground_contact_state() const bool lpos_available = ((time_now_us - _vehicle_local_position.timestamp) < 1_s); - // land speed threshold, 90% of MPC_LAND_SPEED - const float crawl_speed_threshold = 0.9f * math::max(_params.crawlSpeed, 0.1f); - if (lpos_available && _vehicle_local_position.v_z_valid) { // Check if we are moving vertically - this might see a spike after arming due to // throttle-up vibration. If accelerating fast the throttle thresholds will still give // an accurate in-air indication. - float max_climb_rate = math::min(crawl_speed_threshold * 0.5f, _param_lndmc_z_vel_max.get()); + float max_vertical_velocity = _param_lndmc_z_vel_max.get(); if ((time_now_us - _landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) { // Widen acceptance thresholds for landed state right after arming // so that motor spool-up and other effects do not trigger false negatives. - max_climb_rate = _param_lndmc_z_vel_max.get() * 2.5f; + max_vertical_velocity *= 2.5f; } - _vertical_movement = (fabsf(_vehicle_local_position.vz) > max_climb_rate); + _vertical_movement = (fabsf(_vehicle_local_position.vz) > max_vertical_velocity); } else { _vertical_movement = true; @@ -217,7 +202,7 @@ bool MulticopterLandDetector::_get_ground_contact_state() if (_trajectory_setpoint_sub.update(&trajectory_setpoint)) { // Setpoints can be NAN _in_descend = PX4_ISFINITE(trajectory_setpoint.velocity[2]) - && (trajectory_setpoint.velocity[2] >= crawl_speed_threshold); + && (trajectory_setpoint.velocity[2] > FLT_EPSILON); } // ground contact requires commanded descent until landed diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 1d35b98d92..6d73451cd0 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2022 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 @@ -95,8 +95,6 @@ private: param_t minThrottle; param_t hoverThrottle; param_t minManThrottle; - param_t landSpeed; - param_t crawlSpeed; param_t useHoverThrustEstimate; } _paramHandle{}; @@ -104,8 +102,6 @@ private: float minThrottle; float hoverThrottle; float minManThrottle; - float landSpeed; - float crawlSpeed; bool useHoverThrustEstimate; } _params{}; diff --git a/src/modules/land_detector/land_detector_params_mc.c b/src/modules/land_detector/land_detector_params_mc.c index 3c979ac767..d3db8a38c8 100644 --- a/src/modules/land_detector/land_detector_params_mc.c +++ b/src/modules/land_detector/land_detector_params_mc.c @@ -50,14 +50,16 @@ PARAM_DEFINE_FLOAT(LNDMC_TRIG_TIME, 1.0f); /** * Multicopter max climb rate * - * Maximum vertical velocity allowed in the landed state + * Maximum vertical velocity allowed in the landed state. + * Should be set lower than MPC_LAND_SPEED (and MPC_LAND_CRWL + * if distance sensor is present). * * @unit m/s * @decimal 1 * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.50f); +PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.25f); /** * Multicopter max horizontal velocity diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 36769f03c0..3006995d92 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -448,8 +448,9 @@ PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 12.0f); PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 0.7f); /** - * Land crawl descend rate. Used below MPC_LAND_ALT3 if distance - * sensor data is availabe. + * Land crawl descend rate + * + * Used below MPC_LAND_ALT3 if distance sensor data is availabe. * * @unit m/s * @min 0.1