From 740bfc0b3251224ce7d8d2507ec2d406a5048410 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 17 Jul 2016 16:29:02 +0100 Subject: [PATCH] MulticopterLandDetector: use hysteresis lib The hysteresis was not properly implemented in the land detector and is therefore replaced by the library call, both for the freefall detector and the land detector. --- .../land_detector/MulticopterLandDetector.cpp | 31 +++++++++---------- .../land_detector/MulticopterLandDetector.h | 6 ++-- 2 files changed, 18 insertions(+), 19 deletions(-) diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 43b3a935b2..b09823a06e 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -66,10 +66,13 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), _manual{}, _ctrl_state{}, _ctrl_mode{}, - _landTimer(0), - _freefallTimer(0), - _min_trust_start(0) + _min_trust_start(0), + _freefall_hysteresis(false), + _landed_hysteresis(true) { + // Use Trigger time when transitioning from in-air (false) to landed (true). + _landed_hysteresis.set_hysteresis_time_from(false, LAND_DETECTOR_TRIGGER_TIME); + _paramHandle.maxRotation = param_find("LNDMC_ROT_MAX"); _paramHandle.maxVelocity = param_find("LNDMC_XY_VEL_MAX"); _paramHandle.maxClimbRate = param_find("LNDMC_Z_VEL_MAX"); @@ -113,10 +116,13 @@ LandDetectionResult MulticopterLandDetector::update() updateParameterCache(false); - if (get_freefall_state()) { + _landed_hysteresis.set_state_and_update(get_landed_state()); + _freefall_hysteresis.set_state_and_update(get_freefall_state()); + + if (_freefall_hysteresis.get_state()) { _state = LANDDETECTION_RES_FREEFALL; - } else if (get_landed_state()) { + } else if (_landed_hysteresis.get_state()) { _state = LANDDETECTION_RES_LANDED; } else { @@ -133,8 +139,6 @@ bool MulticopterLandDetector::get_freefall_state() return false; } - const uint64_t now = hrt_absolute_time(); - if (_ctrl_state.timestamp == 0) { // _ctrl_state is not valid yet, we have to assume we're not falling. return false; @@ -145,14 +149,7 @@ bool MulticopterLandDetector::get_freefall_state() + _ctrl_state.z_acc * _ctrl_state.z_acc; acc_norm = sqrtf(acc_norm); //norm of specific force. Should be close to 9.8 m/s^2 when landed. - bool freefall = (acc_norm < _params.acc_threshold_m_s2); //true if we are currently falling - - if (!freefall || _freefallTimer == 0) { //reset timer if uav not falling - _freefallTimer = now; - return false; - } - - return (now - _freefallTimer) / 1000000.0f > _params.ff_trigger_time; + return (acc_norm < _params.acc_threshold_m_s2); //true if we are currently falling } bool MulticopterLandDetector::get_landed_state() @@ -240,11 +237,10 @@ bool MulticopterLandDetector::get_landed_state() if (verticalMovement || rotating || !minimalThrust || horizontalMovement) { // Sensed movement or thottle high, so reset the land detector. - _landTimer = now; return false; } - return (now - _landTimer > LAND_DETECTOR_TRIGGER_TIME); + return true; } void MulticopterLandDetector::updateParameterCache(const bool force) @@ -267,6 +263,7 @@ void MulticopterLandDetector::updateParameterCache(const bool force) param_get(_paramHandle.minManThrottle, &_params.minManThrottle); param_get(_paramHandle.acc_threshold_m_s2, &_params.acc_threshold_m_s2); param_get(_paramHandle.ff_trigger_time, &_params.ff_trigger_time); + _freefall_hysteresis.set_hysteresis_time_from(false, 1e6f*_params.ff_trigger_time); } } diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 20bb611cb7..d9e7b11879 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -53,6 +53,7 @@ #include #include #include +#include namespace landdetection { @@ -136,9 +137,10 @@ private: struct control_state_s _ctrl_state; struct vehicle_control_mode_s _ctrl_mode; - uint64_t _landTimer; ///< timestamp in microseconds since a possible land was detected - uint64_t _freefallTimer; ///< timestamp in microseconds since a possible freefall was detected uint64_t _min_trust_start; ///< timestamp when minimum trust was applied first + + systemlib::Hysteresis _freefall_hysteresis; + systemlib::Hysteresis _landed_hysteresis; }; }