diff --git a/src/modules/land_detector/AirshipLandDetector.h b/src/modules/land_detector/AirshipLandDetector.h index 8b218fcca8..64c662a008 100644 --- a/src/modules/land_detector/AirshipLandDetector.h +++ b/src/modules/land_detector/AirshipLandDetector.h @@ -54,6 +54,7 @@ public: protected: bool _get_ground_contact_state() override; bool _get_landed_state() override; + void _set_hysteresis_factor(const int factor) override {}; }; diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index b415de1b0d..b8ca63a0af 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -61,6 +61,7 @@ public: protected: bool _get_landed_state() override; + void _set_hysteresis_factor(const int factor) override {}; private: diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index ca98d577a9..1c0b1f9765 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -96,6 +96,22 @@ void LandDetector::Run() _update_topics(); + if (!_dist_bottom_is_observable) { + // 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; + + } 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; + } + } + const hrt_abstime now_us = hrt_absolute_time(); _freefall_hysteresis.set_state_and_update(_get_freefall_state(), now_us); diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index 2592e00bdd..e4d84562cb 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -136,6 +136,8 @@ protected: */ virtual bool _get_ground_effect_state() { return false; } + virtual void _set_hysteresis_factor(const int factor) = 0; + systemlib::Hysteresis _freefall_hysteresis{false}; systemlib::Hysteresis _landed_hysteresis{true}; systemlib::Hysteresis _maybe_landed_hysteresis{true}; @@ -149,6 +151,7 @@ protected: bool _armed{false}; bool _previous_armed_state{false}; ///< stores the previous actuator_armed.armed state + bool _dist_bottom_is_observable{false}; private: void Run() override; @@ -165,6 +168,8 @@ 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 50c7ec72ab..3d953df8b3 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -82,10 +82,7 @@ MulticopterLandDetector::MulticopterLandDetector() _paramHandle.hoverThrottle = param_find("MPC_THR_HOVER"); // Use Trigger time when transitioning from in-air (false) to landed (true) / ground contact (true). - _freefall_hysteresis.set_hysteresis_time_from(false, FREEFALL_TRIGGER_TIME_US); - _ground_contact_hysteresis.set_hysteresis_time_from(false, GROUND_CONTACT_TRIGGER_TIME_US); - _maybe_landed_hysteresis.set_hysteresis_time_from(false, MAYBE_LAND_DETECTOR_TRIGGER_TIME_US); - _landed_hysteresis.set_hysteresis_time_from(false, LAND_DETECTOR_TRIGGER_TIME_US); + _set_hysteresis_factor(1); } void MulticopterLandDetector::_update_topics() @@ -242,9 +239,14 @@ bool MulticopterLandDetector::_get_ground_contact_state() return true; } + // if there is no distance to ground estimate available then don't enforce using it. + // if a distance to the ground estimate is generally available (_dist_bottom_is_observable=true), then + // we already increased the hysteresis for the land detection states in order to reduce the chance of false positives. + const bool skip_close_to_ground_check = !_dist_bottom_is_observable || !_vehicle_local_position.dist_bottom_valid; // TODO: we need an accelerometer based check for vertical movement for flying without GPS - return ground_contact && !_horizontal_movement && !vertical_movement; + return (_is_close_to_ground() || skip_close_to_ground_check) && ground_contact && !_horizontal_movement + && !vertical_movement; } bool MulticopterLandDetector::_get_maybe_landed_state() @@ -354,4 +356,22 @@ bool MulticopterLandDetector::_get_ground_effect_state() _takeoff_state == takeoff_status_s::TAKEOFF_STATE_RAMPUP; } +bool MulticopterLandDetector::_is_close_to_ground() +{ + if (_vehicle_local_position.dist_bottom_valid) { + return _vehicle_local_position.dist_bottom < DIST_FROM_GROUND_THRESHOLD; + + } else { + return false; + } +} + +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); + _freefall_hysteresis.set_hysteresis_time_from(false, FREEFALL_TRIGGER_TIME_US); +} + } // namespace land_detector diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 32d8bdd5c2..86e4307ab1 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -73,9 +73,12 @@ protected: bool _get_ground_effect_state() override; float _get_max_altitude() override; + + void _set_hysteresis_factor(const int factor) override; private: float _get_gnd_effect_altitude(); + bool _is_close_to_ground(); /** Time in us that freefall has to hold before triggering freefall */ static constexpr hrt_abstime FREEFALL_TRIGGER_TIME_US = 300_ms; @@ -92,6 +95,9 @@ private: /** 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; + /** 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; + /** Handles for interesting parameters. **/ struct { param_t minThrottle; diff --git a/src/modules/land_detector/RoverLandDetector.h b/src/modules/land_detector/RoverLandDetector.h index 6ed05b64c9..8cee7c1d11 100644 --- a/src/modules/land_detector/RoverLandDetector.h +++ b/src/modules/land_detector/RoverLandDetector.h @@ -55,6 +55,7 @@ public: protected: bool _get_ground_contact_state() override; bool _get_landed_state() override; + void _set_hysteresis_factor(const int factor) override {}; };