From 4e744739324d4b75d3240c6381cb0b9da9bd7f1d Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 17 Oct 2022 11:19:19 +0200 Subject: [PATCH] MC LandDetector: remove 2s phase after not maybe landed to still increase thresholds I don't see where this is necessary. During takeoff, the maybe landed flag should only get cleared once system is about to takeoff, and thus well after the spool up is complete (for which the higher thresholds are meant in this case). Signed-off-by: Silvan Fuhrer --- .../land_detector/MulticopterLandDetector.cpp | 16 +++++----------- .../land_detector/MulticopterLandDetector.h | 3 --- 2 files changed, 5 insertions(+), 14 deletions(-) diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 3207a961b5..cba3d049d5 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -148,14 +148,13 @@ bool MulticopterLandDetector::_get_ground_contact_state() if (lpos_available && _vehicle_local_position.v_z_valid) { // Check if we are moving vertically. - // Use wider threshold if currently in "maybe landed" state, or time since then is less - // than LAND_DETECTOR_TAKEOFF_PHASE_TIME_US, as estimation for + // Use wider threshold if currently in "maybe landed" state, as estimation for // vertical speed is often deteriorated when on the ground or due to propeller // up/down throttling. float max_vertical_velocity = _param_lndmc_z_vel_max.get(); - if ((time_now_us - _last_time_maybe_landed) < LAND_DETECTOR_TAKEOFF_PHASE_TIME_US) { + if (_maybe_landed_hysteresis.get_state()) { max_vertical_velocity *= 2.5f; } @@ -267,9 +266,9 @@ bool MulticopterLandDetector::_get_maybe_landed_state() // Next look if vehicle is not rotating (do not consider yaw) float max_rotation_threshold = math::radians(_param_lndmc_rot_max.get()); - // Widen max rotation thresholds if either in maybe landed state or less than - // LAND_DETECTOR_TAKEOFF_PHASE_TIME_US passed since then. - if ((time_now_us - _last_time_maybe_landed) < LAND_DETECTOR_TAKEOFF_PHASE_TIME_US) { + // Widen max rotation thresholds if either in maybe landed state, thus making it harder + // to trigger a false positive !maybe_landed e.g. due to propeller throttling up/down. + if (_maybe_landed_hysteresis.get_state()) { max_rotation_threshold *= 2.5f; } @@ -294,11 +293,6 @@ bool MulticopterLandDetector::_get_maybe_landed_state() bool MulticopterLandDetector::_get_landed_state() { - // update _last_time_maybe_landed as long as the vehicle is maybe landed - if (_maybe_landed_hysteresis.get_state()) { - _last_time_maybe_landed = hrt_absolute_time(); - } - // When not armed, consider to be landed if (!_armed) { return true; diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index c088ba954b..1e473eff83 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -84,9 +84,6 @@ private: /** Time in us that freefall has to hold before triggering freefall */ static constexpr hrt_abstime FREEFALL_TRIGGER_TIME_US = 300_ms; - /** Time interval in us in which wider acceptance thresholds are used after the "maybe landed" is cleared before takeoff. */ - static constexpr hrt_abstime LAND_DETECTOR_TAKEOFF_PHASE_TIME_US = 2_s; - /** 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;