From 2e292abfffb8958593f9addb435241526eb6cecb Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 21 Apr 2021 19:24:12 +0200 Subject: [PATCH] MulticopterLandDetector: Make land detection time configurable The tree stages used arbitrary 350, 250 300ms totally 900ms So this changes it to each stage to a third of the parameter. Default it is 1 second -> 333ms per stage. --- src/modules/land_detector/AirshipLandDetector.h | 1 - src/modules/land_detector/LandDetector.cpp | 14 ++++++-------- src/modules/land_detector/LandDetector.h | 2 -- .../land_detector/MulticopterLandDetector.cpp | 9 +++------ .../land_detector/MulticopterLandDetector.h | 10 +--------- src/modules/land_detector/RoverLandDetector.h | 1 - .../land_detector/land_detector_params_mc.c | 16 ++++++++++++++++ 7 files changed, 26 insertions(+), 27 deletions(-) diff --git a/src/modules/land_detector/AirshipLandDetector.h b/src/modules/land_detector/AirshipLandDetector.h index 64c662a008..abbd693ae8 100644 --- a/src/modules/land_detector/AirshipLandDetector.h +++ b/src/modules/land_detector/AirshipLandDetector.h @@ -55,7 +55,6 @@ protected: bool _get_ground_contact_state() override; bool _get_landed_state() override; void _set_hysteresis_factor(const int factor) override {}; - }; } // namespace land_detector diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index a4cac6f101..1f4eb50442 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -100,16 +100,14 @@ void LandDetector::Run() // we consider the distance to the ground observable if the system is using a range sensor _dist_bottom_is_observable = _vehicle_local_position.dist_bottom_sensor_bitfield & vehicle_local_position_s::DIST_BOTTOM_SENSOR_RANGE; + } + + // Increase land detection time if not close to ground + if (_dist_bottom_is_observable && !_vehicle_local_position.dist_bottom_valid) { + _set_hysteresis_factor(3); } else { - if (!_high_hysteresis_active && !_vehicle_local_position.dist_bottom_valid) { - _set_hysteresis_factor(3); - _high_hysteresis_active = true; - - } else if (_high_hysteresis_active && _vehicle_local_position.dist_bottom_valid) { - _set_hysteresis_factor(1); - _high_hysteresis_active = false; - } + _set_hysteresis_factor(1); } const hrt_abstime now_us = hrt_absolute_time(); diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index 129a209991..09b502dfa9 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -173,8 +173,6 @@ private: hrt_abstime _takeoff_time{0}; hrt_abstime _total_flight_time{0}; ///< total vehicle flight time in microseconds - bool _high_hysteresis_active{false}; - perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; uORB::Publication _vehicle_land_detected_pub{ORB_ID(vehicle_land_detected)}; diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index b15c65ff6c..0aade86b48 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -80,9 +80,6 @@ MulticopterLandDetector::MulticopterLandDetector() _paramHandle.minThrottle = param_find("MPC_THR_MIN"); _paramHandle.useHoverThrustEstimate = param_find("MPC_USE_HTE"); _paramHandle.hoverThrottle = param_find("MPC_THR_HOVER"); - - // Use Trigger time when transitioning from in-air (false) to landed (true) / ground contact (true). - _set_hysteresis_factor(1); } void MulticopterLandDetector::_update_topics() @@ -370,9 +367,9 @@ bool MulticopterLandDetector::_is_close_to_ground() void MulticopterLandDetector::_set_hysteresis_factor(const int factor) { - _ground_contact_hysteresis.set_hysteresis_time_from(false, GROUND_CONTACT_TRIGGER_TIME_US * factor); - _landed_hysteresis.set_hysteresis_time_from(false, LAND_DETECTOR_TRIGGER_TIME_US * factor); - _maybe_landed_hysteresis.set_hysteresis_time_from(false, MAYBE_LAND_DETECTOR_TRIGGER_TIME_US * factor); + _ground_contact_hysteresis.set_hysteresis_time_from(false, _param_lndmc_trig_time.get() * 1_s / 3 * factor); + _landed_hysteresis.set_hysteresis_time_from(false, _param_lndmc_trig_time.get() * 1_s / 3 * factor); + _maybe_landed_hysteresis.set_hysteresis_time_from(false, _param_lndmc_trig_time.get() * 1_s / 3 * factor); _freefall_hysteresis.set_hysteresis_time_from(false, FREEFALL_TRIGGER_TIME_US); } diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index ede50711df..e6ed259715 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -87,15 +87,6 @@ private: /** Time in us that freefall has to hold before triggering freefall */ static constexpr hrt_abstime FREEFALL_TRIGGER_TIME_US = 300_ms; - /** Time in us that ground contact condition have to hold before triggering contact ground */ - static constexpr hrt_abstime GROUND_CONTACT_TRIGGER_TIME_US = 350_ms; - - /** Time in us that almost landing conditions have to hold before triggering almost landed . */ - static constexpr hrt_abstime MAYBE_LAND_DETECTOR_TRIGGER_TIME_US = 250_ms; - - /** Time in us that landing conditions have to hold before triggering a land. */ - static constexpr hrt_abstime LAND_DETECTOR_TRIGGER_TIME_US = 300_ms; - /** Time interval in us in which wider acceptance thresholds are used after landed. */ static constexpr hrt_abstime LAND_DETECTOR_LAND_PHASE_TIME_US = 2_s; @@ -148,6 +139,7 @@ private: DEFINE_PARAMETERS_CUSTOM_PARENT( LandDetector, + (ParamFloat) _param_lndmc_trig_time, (ParamFloat) _param_lndmc_alt_max, (ParamFloat) _param_lndmc_rot_max, (ParamFloat) _param_lndmc_xy_vel_max, diff --git a/src/modules/land_detector/RoverLandDetector.h b/src/modules/land_detector/RoverLandDetector.h index 8cee7c1d11..5ebc5b6677 100644 --- a/src/modules/land_detector/RoverLandDetector.h +++ b/src/modules/land_detector/RoverLandDetector.h @@ -56,7 +56,6 @@ protected: bool _get_ground_contact_state() override; bool _get_landed_state() override; void _set_hysteresis_factor(const int factor) override {}; - }; } // namespace land_detector diff --git a/src/modules/land_detector/land_detector_params_mc.c b/src/modules/land_detector/land_detector_params_mc.c index 465de6e580..cdfe6d5c76 100644 --- a/src/modules/land_detector/land_detector_params_mc.c +++ b/src/modules/land_detector/land_detector_params_mc.c @@ -31,6 +31,22 @@ * ****************************************************************************/ +/** + * Multicopter land detection trigger time + * + * Total time it takes to go through all three land detection stages: + * ground contact, maybe landed, landed + * when all necessary conditions are constantly met. + * + * @unit s + * @min 0.1 + * @max 10.0 + * @decimal 1 + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LNDMC_TRIG_TIME, 1.0f); + /** * Multicopter max climb rate *