From 68c171fd4f6122db5050602c4a8ef505f769050e Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 6 Mar 2021 22:19:37 -0500 Subject: [PATCH] land_detector: continue respecting hover thrust throughout descent --- src/modules/land_detector/MulticopterLandDetector.cpp | 10 ++++++++-- src/modules/land_detector/MulticopterLandDetector.h | 1 + 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index a050ac5f4d..d170cfb2b3 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -161,7 +161,6 @@ bool MulticopterLandDetector::_get_ground_contact_state() const hrt_abstime time_now_us = hrt_absolute_time(); const bool lpos_available = ((time_now_us - _vehicle_local_position.timestamp) < 1_s); - const bool hover_thrust_estimate_valid = ((time_now_us - _hover_thrust_estimate_last_valid) < 1_s); // land speed threshold, 90% of MPC_LAND_SPEED const float land_speed_threshold = 0.9f * math::max(_params.landSpeed, 0.1f); @@ -200,8 +199,15 @@ bool MulticopterLandDetector::_get_ground_contact_state() _below_gnd_effect_hgt = false; } + const bool hover_thrust_estimate_valid = ((time_now_us - _hover_thrust_estimate_last_valid) < 1_s); + + if (!_in_descend || hover_thrust_estimate_valid) { + // continue using valid hover thrust if it became invalid during descent + _hover_thrust_estimate_valid = hover_thrust_estimate_valid; + } + // low thrust: 30% of throttle range between min and hover, relaxed to 60% if hover thrust estimate available - const float thr_pct_hover = hover_thrust_estimate_valid ? 0.6f : 0.3f; + const float thr_pct_hover = _hover_thrust_estimate_valid ? 0.6f : 0.3f; const float sys_low_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * thr_pct_hover; bool ground_contact = (_actuator_controls_throttle <= sys_low_throttle); diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 5715b2f807..32d8bdd5c2 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -117,6 +117,7 @@ private: uORB::Subscription _takeoff_status_sub{ORB_ID(takeoff_status)}; hrt_abstime _hover_thrust_estimate_last_valid{0}; + bool _hover_thrust_estimate_valid{false}; bool _flag_control_climb_rate_enabled{false}; bool _hover_thrust_initialized{false};