From 2f164602b429fd509f489f50147c780037ef2362 Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Tue, 17 Jan 2017 17:28:50 +0100 Subject: [PATCH] LandDetector: - constructor initalization fix - set trigger time for ground contact hysteresis - updated ground_contact_state logic MulticopterLandDetector: - added hysteresis for ground_contact VtolLandDetector: - get_ground_contact_state function that return the one form MultcopterLandDetector FixedWingLandDetector: - get_ground_contact_state with a return false: requires implementation --- .../land_detector/FixedwingLandDetector.cpp | 5 ++++ .../land_detector/FixedwingLandDetector.h | 2 ++ src/modules/land_detector/LandDetector.cpp | 25 ++++++++++++++++--- src/modules/land_detector/LandDetector.h | 9 +++++-- .../land_detector/MulticopterLandDetector.cpp | 2 +- .../land_detector/MulticopterLandDetector.h | 2 +- src/modules/land_detector/VtolLandDetector.h | 2 +- 7 files changed, 39 insertions(+), 8 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 0721e68e8e..ed1fc8ee72 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -97,6 +97,11 @@ bool FixedwingLandDetector::_get_freefall_state() // TODO return false; } +bool FixedwingLandDetector::_get_ground_contact_state(){ + + // TODO + return false; +} bool FixedwingLandDetector::_get_landed_state() { diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index e52ce1d95e..7c963fce32 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_ground_contact_state() override; + virtual bool _get_freefall_state() override; private: struct { diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 5ed6961b59..aa18afc8b7 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -52,14 +52,18 @@ namespace land_detector LandDetector::LandDetector() : _landDetectedPub(nullptr), _landDetected{0, false, false}, + _parameterSub(0), + _state{}, _freefall_hysteresis(false), _landed_hysteresis(true), + _ground_contact_hysteresis(false), _taskShouldExit(false), _taskIsRunning(false), _work{} { - // Use Trigger time when transitioning from in-air (false) to landed (true). + // 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); + _ground_contact_hysteresis.set_hysteresis_time_from(false, GROUND_CONTACT_TRIGGER_TIME_US); } LandDetector::~LandDetector() @@ -98,6 +102,7 @@ void LandDetector::_cycle() _landDetected.timestamp = hrt_absolute_time(); _landDetected.landed = false; _landDetected.freefall = false; + _landDetected.ground_contact = false; // Initialize uORB topics. _initialize_topics(); @@ -116,15 +121,18 @@ void LandDetector::_cycle() bool landDetected = (_state == LandDetectionState::LANDED); bool freefallDetected = (_state == LandDetectionState::FREEFALL); + bool groundContactDetected = ( _state == LandDetectionState::GROUND_CONTACT); // Only publish very first time or when the result has changed. if ((_landDetectedPub == nullptr) || (_landDetected.landed != landDetected) || - (_landDetected.freefall != freefallDetected)) { + (_landDetected.freefall != freefallDetected) || + (_landDetected.ground_contact != groundContactDetected)) { _landDetected.timestamp = hrt_absolute_time(); _landDetected.landed = (_state == LandDetectionState::LANDED); _landDetected.freefall = (_state == LandDetectionState::FREEFALL); + _landDetected.ground_contact = (_state == LandDetectionState::GROUND_CONTACT); int instance; orb_publish_auto(ORB_ID(vehicle_land_detected), &_landDetectedPub, &_landDetected, @@ -160,15 +168,26 @@ void LandDetector::_check_params(const bool force) void LandDetector::_update_state() { + /* ground contact and landed can be true simultaneously but only one state can be true at a particular time + * with higher priority for landed */ + bool groundContact = _get_ground_contact_state(); bool landed = _get_landed_state(); + bool freefall = _get_freefall_state(); + + _ground_contact_hysteresis.set_state_and_update(groundContact); _landed_hysteresis.set_state_and_update(landed); - _freefall_hysteresis.set_state_and_update(_get_freefall_state()); + _freefall_hysteresis.set_state_and_update(freefall); if (_freefall_hysteresis.get_state()) { _state = LandDetectionState::FREEFALL; } else if (_landed_hysteresis.get_state()) { _state = LandDetectionState::LANDED; + /* need to set ground_contact_state to false */ + _ground_contact_hysteresis.set_state_and_update(false); + + } else if (_ground_contact_hysteresis.get_state()){ + _state = LandDetectionState::GROUND_CONTACT; } else { _state = LandDetectionState::FLYING; diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index cfe5c87179..717106e400 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -116,7 +116,7 @@ protected: /** * @return true if UAV is touching ground but not landed */ - bool _get_ground_contact_state() { return _get_landed_state(); } + virtual bool _get_ground_contact_state() = 0; /** * @return true if UAV is in free-fall state. @@ -134,7 +134,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 = 2000000; + static constexpr uint64_t 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; /** Time interval in us in which wider acceptance thresholds are used after arming. */ static constexpr uint64_t LAND_DETECTOR_ARM_PHASE_TIME_US = 2000000; @@ -148,6 +151,8 @@ protected: systemlib::Hysteresis _freefall_hysteresis; systemlib::Hysteresis _landed_hysteresis; + systemlib::Hysteresis _ground_contact_hysteresis; + private: static void _cycle_trampoline(void *arg); diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index dde19c3998..f053dfa243 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -112,7 +112,7 @@ void MulticopterLandDetector::_update_params() param_get(_paramHandle.minManThrottle, &_params.minManThrottle); param_get(_paramHandle.freefall_acc_threshold, &_params.freefall_acc_threshold); param_get(_paramHandle.freefall_trigger_time, &_params.freefall_trigger_time); - _freefall_hysteresis.set_hysteresis_time_from(false, 1e6f * _params.freefall_trigger_time); + _freefall_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(1e6f * _params.freefall_trigger_time)); } diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index a1e3e1e4f8..0240c592d1 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -71,7 +71,7 @@ protected: virtual bool _get_landed_state() override; - bool _get_ground_contact_state(); + virtual bool _get_ground_contact_state() override; virtual bool _get_freefall_state() override; private: diff --git a/src/modules/land_detector/VtolLandDetector.h b/src/modules/land_detector/VtolLandDetector.h index 4a0dd4faf0..480da643aa 100644 --- a/src/modules/land_detector/VtolLandDetector.h +++ b/src/modules/land_detector/VtolLandDetector.h @@ -62,7 +62,7 @@ protected: virtual bool _get_landed_state() override; - bool _get_ground_contact_state(); + virtual bool _get_ground_contact_state() override; virtual bool _get_freefall_state() override;