From f8e9f380d073c88504bca72aaa94386ff9cec966 Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Wed, 24 May 2017 16:42:14 +0200 Subject: [PATCH] landdetector: add additional landdetection state --- msg/vehicle_land_detected.msg | 3 +- .../land_detector/FixedwingLandDetector.cpp | 7 ++++ .../land_detector/FixedwingLandDetector.h | 2 ++ src/modules/land_detector/LandDetector.cpp | 10 ++++++ src/modules/land_detector/LandDetector.h | 14 ++++++-- .../land_detector/MulticopterLandDetector.cpp | 35 ++++++++++++++++--- .../land_detector/MulticopterLandDetector.h | 3 ++ .../land_detector/VtolLandDetector.cpp | 7 ++++ src/modules/land_detector/VtolLandDetector.h | 2 ++ 9 files changed, 76 insertions(+), 7 deletions(-) diff --git a/msg/vehicle_land_detected.msg b/msg/vehicle_land_detected.msg index 6ab67c5081..387d18f0eb 100644 --- a/msg/vehicle_land_detected.msg +++ b/msg/vehicle_land_detected.msg @@ -1,4 +1,5 @@ bool landed # true if vehicle is currently landed on the ground bool freefall # true if vehicle is currently in free-fall bool ground_contact # true if vehicle has ground contact but is not landed -float32 alt_max # maximum altitude in [m] that can be reached \ No newline at end of file +bool maybe_landed # true if the vehicle might have landed +float32 alt_max # maximum altitude in [m] that can be reached diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index eefab8f5b3..acab306203 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -112,6 +112,13 @@ bool FixedwingLandDetector::_get_ground_contact_state() return false; } +bool FixedwingLandDetector::_get_maybe_landed_state() +{ + + // TODO + return false; +} + bool FixedwingLandDetector::_get_landed_state() { // only trigger flight conditions if we are armed diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index 299b46590c..dfcd847f46 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -65,6 +65,8 @@ protected: virtual bool _get_landed_state() override; + virtual bool _get_maybe_landed_state() override; + virtual bool _get_ground_contact_state() override; virtual bool _get_freefall_state() override; diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 77862203e1..594104475e 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -57,6 +57,7 @@ LandDetector::LandDetector() : _state{}, _freefall_hysteresis(false), _landed_hysteresis(true), + _maybe_landed_hysteresis(true), _ground_contact_hysteresis(true), _total_flight_time{0}, _takeoff_time{0}, @@ -64,6 +65,7 @@ LandDetector::LandDetector() : { // Use Trigger time when transitioning from in-air (false) to landed (true) / ground contact (true). _landed_hysteresis.set_hysteresis_time_from(false, LAND_DETECTOR_TRIGGER_TIME_US); + _maybe_landed_hysteresis.set_hysteresis_time_from(false, MAYBE_LAND_DETECTOR_TRIGGER_TIME_US); _ground_contact_hysteresis.set_hysteresis_time_from(false, GROUND_CONTACT_TRIGGER_TIME_US); } @@ -93,6 +95,7 @@ void LandDetector::_cycle() _landDetected.freefall = false; _landDetected.landed = false; _landDetected.ground_contact = false; + _landDetected.maybe_landed = false; _p_total_flight_time_high = param_find("LND_FLIGHT_T_HI"); _p_total_flight_time_low = param_find("LND_FLIGHT_T_LO"); @@ -117,6 +120,7 @@ void LandDetector::_cycle() bool freefallDetected = (_state == LandDetectionState::FREEFALL); bool landDetected = (_state == LandDetectionState::LANDED); + bool maybe_landedDetected = (_state == LandDetectionState::MAYBE_LANDED); bool ground_contactDetected = (_state == LandDetectionState::GROUND_CONTACT); // Only publish very first time or when the result has changed. @@ -124,6 +128,7 @@ void LandDetector::_cycle() (_landDetected.freefall != freefallDetected) || (_landDetected.landed != landDetected) || (_landDetected.ground_contact != ground_contactDetected) || + (_landDetected.maybe_landed != maybe_landedDetected) || (fabsf(_landDetected.alt_max - alt_max_prev) > FLT_EPSILON)) { if (!landDetected && _landDetected.landed) { @@ -144,6 +149,7 @@ void LandDetector::_cycle() _landDetected.freefall = (_state == LandDetectionState::FREEFALL); _landDetected.landed = (_state == LandDetectionState::LANDED); _landDetected.ground_contact = (_state == LandDetectionState::GROUND_CONTACT); + _landDetected.maybe_landed = (_state == LandDetectionState::MAYBE_LANDED); _landDetected.alt_max = _altitude_max; int instance; @@ -188,6 +194,7 @@ void LandDetector::_update_state() * with higher priority for landed */ _freefall_hysteresis.set_state_and_update(_get_freefall_state()); _landed_hysteresis.set_state_and_update(_get_landed_state()); + _maybe_landed_hysteresis.set_state_and_update(_get_maybe_landed_state()); _ground_contact_hysteresis.set_state_and_update(_landed_hysteresis.get_state() || _get_ground_contact_state()); if (_freefall_hysteresis.get_state()) { @@ -196,6 +203,9 @@ void LandDetector::_update_state() } else if (_landed_hysteresis.get_state()) { _state = LandDetectionState::LANDED; + } else if (_maybe_landed_hysteresis.get_state()) { + _state = LandDetectionState::MAYBE_LANDED; + } else if (_ground_contact_hysteresis.get_state()) { _state = LandDetectionState::GROUND_CONTACT; diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index 87341a07ca..fc59327c3f 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -60,7 +60,8 @@ public: FLYING = 0, LANDED = 1, FREEFALL = 2, - GROUND_CONTACT = 3 + GROUND_CONTACT = 3, + MAYBE_LANDED = 4 }; LandDetector(); @@ -115,6 +116,11 @@ protected: */ virtual bool _get_landed_state() = 0; + /** + * @return true if UAV is in almost landed state + */ + virtual bool _get_maybe_landed_state() = 0; + /** * @return true if UAV is touching ground but not landed */ @@ -141,7 +147,10 @@ protected: static constexpr uint32_t LAND_DETECTOR_UPDATE_RATE_HZ = 50; /** Time in us that landing conditions have to hold before triggering a land. */ - static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME_US = 1500000; + static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME_US = 500000; + + /** Time in us that almost landing conditions have to hold before triggering almost landed . */ + static constexpr uint64_t MAYBE_LAND_DETECTOR_TRIGGER_TIME_US = 1500000; /** Time in us that ground contact condition have to hold before triggering contact ground */ static constexpr uint64_t GROUND_CONTACT_TRIGGER_TIME_US = 1000000; @@ -158,6 +167,7 @@ protected: systemlib::Hysteresis _freefall_hysteresis; systemlib::Hysteresis _landed_hysteresis; + systemlib::Hysteresis _maybe_landed_hysteresis; systemlib::Hysteresis _ground_contact_hysteresis; float _altitude_max; diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 88a33265a0..295d5e9854 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -186,10 +186,15 @@ bool MulticopterLandDetector::_get_ground_contact_state() // If pilots commands down or in auto mode and we are already below minimal thrust and we do not move down we assume ground contact // TODO: we need an accelerometer based check for vertical movement for flying without GPS - return manual_control_idle_or_auto && _has_minimal_thrust() && (!verticalMovement || !_has_altitude_lock()); + if (manual_control_idle_or_auto && _has_low_thrust() && + (!verticalMovement || !_has_altitude_lock())) { + return true; + } + + return false; } -bool MulticopterLandDetector::_get_landed_state() +bool MulticopterLandDetector::_get_maybe_landed_state() { // Time base for this function const uint64_t now = hrt_absolute_time(); @@ -257,6 +262,17 @@ bool MulticopterLandDetector::_get_landed_state() return false; } +bool MulticopterLandDetector::_get_landed_state() +{ + // if we have maybe_landed, the mc_pos_control goes into idle (thrust_sp = 0.0) + // therefore check if all other condition of the landed state remain true + if (_maybe_landed_hysteresis.get_state()) { + return true; + } + + return false; +} + float MulticopterLandDetector::_get_takeoff_throttle() { /* Position mode */ @@ -314,9 +330,21 @@ bool MulticopterLandDetector::_has_manual_control_present() return _control_mode.flag_control_manual_enabled && _manual.timestamp > 0; } +bool MulticopterLandDetector::_has_low_thrust() +{ + // 30% of throttle range between min and hover + float sys_min_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * 0.3f; + + PX4_INFO("_actuatl control 3: %.5f, sys_min_throttle: %.5f", (double)_actuators.control[3], (double)sys_min_throttle); + + // Check if thrust output is less than the minimum auto throttle param. + return _actuators.control[3] <= sys_min_throttle; + +} + bool MulticopterLandDetector::_has_minimal_thrust() { - // 10% of throttle range between min and hover + // 10% of throttle range between min and hover once we entered ground contact float sys_min_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * _params.throttleRange; // Determine the system min throttle based on flight mode @@ -328,5 +356,4 @@ bool MulticopterLandDetector::_has_minimal_thrust() return _actuators.control[3] <= sys_min_throttle; } - } diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 2579280cc7..ced060caac 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -74,6 +74,8 @@ protected: virtual bool _get_ground_contact_state() override; + virtual bool _get_maybe_landed_state() override; + virtual bool _get_freefall_state() override; virtual float _get_max_altitude() override; @@ -139,6 +141,7 @@ private: bool _has_position_lock(); bool _has_manual_control_present(); bool _has_minimal_thrust(); + bool _has_low_thrust(); }; diff --git a/src/modules/land_detector/VtolLandDetector.cpp b/src/modules/land_detector/VtolLandDetector.cpp index 6cb74e205f..4c918b4311 100644 --- a/src/modules/land_detector/VtolLandDetector.cpp +++ b/src/modules/land_detector/VtolLandDetector.cpp @@ -76,6 +76,13 @@ bool VtolLandDetector::_get_ground_contact_state() return MulticopterLandDetector::_get_ground_contact_state(); } +bool VtolLandDetector::_get_maybe_landed_state() +{ + + // TODO + return false; +} + bool VtolLandDetector::_get_landed_state() { // this is returned from the mutlicopter land detector diff --git a/src/modules/land_detector/VtolLandDetector.h b/src/modules/land_detector/VtolLandDetector.h index 480da643aa..f7f1080721 100644 --- a/src/modules/land_detector/VtolLandDetector.h +++ b/src/modules/land_detector/VtolLandDetector.h @@ -62,6 +62,8 @@ protected: virtual bool _get_landed_state() override; + virtual bool _get_maybe_landed_state() override; + virtual bool _get_ground_contact_state() override; virtual bool _get_freefall_state() override;