From 87471a988da8b485446558d65fba5f6b4531f781 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 13 Sep 2020 14:12:18 -0400 Subject: [PATCH] land_detector: use hover thrust if it was valid recently - the hover thrust estimate will often invalidate upon ground contact, but before the land detector ground_contact triggers - use same time throughout call to avoid subtle timing surprises --- .../land_detector/MulticopterLandDetector.cpp | 21 +++++++++++-------- .../land_detector/MulticopterLandDetector.h | 3 ++- 2 files changed, 14 insertions(+), 10 deletions(-) diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index b54ac8f5d4..dabef6bbfe 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -108,9 +108,8 @@ void MulticopterLandDetector::_update_topics() if (_hover_thrust_estimate_sub.update(&hte)) { if (hte.valid) { _params.hoverThrottle = hte.hover_thrust; + _hover_thrust_estimate_last_valid = hte.timestamp; } - - _hover_thrust_estimate_valid = hte.valid; } } } @@ -153,7 +152,10 @@ bool MulticopterLandDetector::_get_freefall_state() bool MulticopterLandDetector::_get_ground_contact_state() { - const bool lpos_available = (hrt_elapsed_time(&_vehicle_local_position.timestamp) < 1_s); + 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); @@ -166,7 +168,7 @@ bool MulticopterLandDetector::_get_ground_contact_state() // an accurate in-air indication. float max_climb_rate = math::min(land_speed_threshold * 0.5f, _param_lndmc_z_vel_max.get()); - if (hrt_elapsed_time(&_landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) { + 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; @@ -187,7 +189,7 @@ bool MulticopterLandDetector::_get_ground_contact_state() // 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); @@ -234,6 +236,7 @@ bool MulticopterLandDetector::_get_maybe_landed_state() return true; } + const hrt_abstime time_now_us = hrt_absolute_time(); // minimal throttle: initially 10% of throttle range between min and hover float sys_min_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * 0.1f; @@ -246,7 +249,7 @@ bool MulticopterLandDetector::_get_maybe_landed_state() // Check if thrust output is less than the minimum throttle. if (_actuator_controls_throttle <= sys_min_throttle) { if (_min_thrust_start == 0) { - _min_thrust_start = hrt_absolute_time(); + _min_thrust_start = time_now_us; } } else { @@ -263,7 +266,7 @@ bool MulticopterLandDetector::_get_maybe_landed_state() float landThresholdFactor = 1.f; // Widen acceptance thresholds for landed state right after landed - if (hrt_elapsed_time(&_landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) { + if ((time_now_us - _landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) { landThresholdFactor = 2.5f; } @@ -278,12 +281,12 @@ bool MulticopterLandDetector::_get_maybe_landed_state() } // If vertical velocity is available: ground contact, no thrust, no movement -> landed - if ((hrt_elapsed_time(&_vehicle_local_position.timestamp) < 1_s) && _vehicle_local_position.v_z_valid) { + if (((time_now_us - _vehicle_local_position.timestamp) < 1_s) && _vehicle_local_position.v_z_valid) { return _ground_contact_hysteresis.get_state(); } // Otherwise, landed if the system has minimum thrust (manual or in failsafe) and no rotation for at least 8 seconds - return (_min_thrust_start > 0) && (hrt_elapsed_time(&_min_thrust_start) > 8_s); + return (_min_thrust_start > 0) && ((time_now_us - _min_thrust_start) > 8_s); } bool MulticopterLandDetector::_get_landed_state() diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index a81c306ea5..6b1576da25 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -112,9 +112,10 @@ private: uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)}; + hrt_abstime _hover_thrust_estimate_last_valid{0}; + bool _flag_control_climb_rate_enabled{false}; bool _hover_thrust_initialized{false}; - bool _hover_thrust_estimate_valid{false}; float _actuator_controls_throttle{0.f};