From 480dd0922b65a00a8e5424d475b5b32c57b27bc2 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 25 Jan 2017 11:41:13 +0100 Subject: [PATCH] Land detector: revision of the 2 stage landing mechanism Ground detect: pilot want down or we are on minimum thrust by auto land but no vertical movement -> Controller should relax x,y corrections and even ramp down desired thrust Landed: All other conditions are eventually met --- src/modules/land_detector/LandDetector.cpp | 25 ++- .../land_detector/MulticopterLandDetector.cpp | 147 ++++++++++-------- .../land_detector/MulticopterLandDetector.h | 9 +- 3 files changed, 103 insertions(+), 78 deletions(-) diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 650be19a62..9ae623639e 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -56,7 +56,7 @@ LandDetector::LandDetector() : _state{}, _freefall_hysteresis(false), _landed_hysteresis(true), - _ground_contact_hysteresis(false), + _ground_contact_hysteresis(true), _taskShouldExit(false), _taskIsRunning(false), _work{} @@ -100,8 +100,8 @@ void LandDetector::_cycle() if (!_taskIsRunning) { // Advertise the first land detected uORB. _landDetected.timestamp = hrt_absolute_time(); - _landDetected.landed = false; _landDetected.freefall = false; + _landDetected.landed = false; _landDetected.ground_contact = false; // Initialize uORB topics. @@ -119,19 +119,20 @@ void LandDetector::_cycle() _update_state(); - bool landDetected = (_state == LandDetectionState::LANDED); bool freefallDetected = (_state == LandDetectionState::FREEFALL); - bool groundContactDetected = (_state == LandDetectionState::GROUND_CONTACT); + bool landDetected = (_state == LandDetectionState::LANDED); + bool ground_contactDetected = (_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.ground_contact != groundContactDetected)) { + (_landDetected.landed != landDetected) || + (_landDetected.ground_contact != ground_contactDetected)) { _landDetected.timestamp = hrt_absolute_time(); - _landDetected.landed = (_state == LandDetectionState::LANDED); _landDetected.freefall = (_state == LandDetectionState::FREEFALL); + _landDetected.landed = (_state == LandDetectionState::LANDED); _landDetected.ground_contact = (_state == LandDetectionState::GROUND_CONTACT); int instance; @@ -170,21 +171,19 @@ 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(); + bool landed = _get_landed_state(); + bool groundContact = (landed || _get_ground_contact_state()); - _ground_contact_hysteresis.set_state_and_update(groundContact); - _landed_hysteresis.set_state_and_update(landed); _freefall_hysteresis.set_state_and_update(freefall); + _landed_hysteresis.set_state_and_update(landed); + _ground_contact_hysteresis.set_state_and_update(groundContact); 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; diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 2375441c26..c9d43d6d9c 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -74,7 +74,7 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), _paramHandle.maxClimbRate = param_find("LNDMC_Z_VEL_MAX"); _paramHandle.throttleRange = param_find("LNDMC_THR_RANGE"); _paramHandle.minThrottle = param_find("MPC_THR_MIN"); - _paramHandle.hoverThrottleAuto = param_find("MPC_THR_HOVER"); + _paramHandle.hoverThrottle = param_find("MPC_THR_HOVER"); _paramHandle.minManThrottle = param_find("MPC_MANTHR_MIN"); _paramHandle.freefall_acc_threshold = param_find("LNDMC_FFALL_THR"); _paramHandle.freefall_trigger_time = param_find("LNDMC_FFALL_TTRI"); @@ -111,7 +111,7 @@ void MulticopterLandDetector::_update_params() param_get(_paramHandle.maxRotation, &_params.maxRotation_rad_s); _params.maxRotation_rad_s = math::radians(_params.maxRotation_rad_s); param_get(_paramHandle.minThrottle, &_params.minThrottle); - param_get(_paramHandle.hoverThrottleAuto, &_params.hoverThrottleAuto); + param_get(_paramHandle.hoverThrottle, &_params.hoverThrottle); param_get(_paramHandle.throttleRange, &_params.throttleRange); param_get(_paramHandle.minManThrottle, &_params.minManThrottle); param_get(_paramHandle.freefall_acc_threshold, &_params.freefall_acc_threshold); @@ -145,57 +145,76 @@ bool MulticopterLandDetector::_get_ground_contact_state() // Time base for this function const uint64_t now = hrt_absolute_time(); - // 10% of throttle range between min and hover - float sys_min_throttle = _params.minThrottle + (_params.hoverThrottleAuto - _params.minThrottle) * - _params.throttleRange; + // only trigger flight conditions if we are armed + if (!_arming.armed) { + _arming_time = 0; + return true; - // Determine the system min throttle based on flight mode - if (!_control_mode.flag_control_altitude_enabled) { - sys_min_throttle = (_params.minManThrottle + 0.01f); + } else if (_arming_time == 0) { + _arming_time = now; } - // Check if thrust output is less than the minimum auto throttle param. - bool minimalThrust = (_actuators.control[3] <= sys_min_throttle); + // If in manual flight mode never report landed if the user has more than idle throttle + // Check if user commands throttle and if so, report no ground contact based on + // the user intent to take off (even if the system might physically still have + // ground contact at this point). + const bool manual_control_move_down = _get_manual_control_present() && _manual.z < 0.05f; - if (minimalThrust && _min_trust_start == 0) { - _min_trust_start = now; + // Widen acceptance thresholds for landed state right after arming + // so that motor spool-up and other effects do not trigger false negatives. + float armThresholdFactor = 1.0f; + if (hrt_elapsed_time(&_arming_time) < LAND_DETECTOR_ARM_PHASE_TIME_US) { + armThresholdFactor = 2.5f; + } - } else if (!minimalThrust) { - _min_trust_start = 0; + // Check if we are moving vertically - this might see a spike after arming due to + // throttle-up vibration. If accelerating fast the throttle thresholds will still give + // an accurate in-air indication. + bool verticalMovement = fabsf(_vehicleLocalPosition.vz) > _params.maxClimbRate * armThresholdFactor; + + // If pilots commands fully down or already below minimal thrust because of auto land and we do not move down we assume ground contact + // TODO: we need an accelerometer based check for vertical movement for flying without GPS + if ((manual_control_move_down || _get_minimal_thrust()) && + (!verticalMovement || !_get_position_lock_available())) { + return true; } + return false; +} + +bool MulticopterLandDetector::_get_landed_state() +{ + // Time base for this function + const uint64_t now = hrt_absolute_time(); + // only trigger flight conditions if we are armed if (!_arming.armed) { - _arming_time = 0; - return true; - - } else if (_arming_time == 0) { - _arming_time = now; } - const bool manual_control_present = _control_mode.flag_control_manual_enabled && _manual.timestamp > 0; + // If we control manually and are still landed, we want to stay idle until the pilot rises the throttle for takeoff + if (_state == LandDetectionState::LANDED && _get_manual_control_present()) { + if (_manual.z < _get_takeoff_throttle()) { + return true; - // If we control manually and are still landed, we want to stay idle until the pilot rises the throttle - if (_state == LandDetectionState::LANDED && manual_control_present && _manual.z < get_takeoff_throttle()) { - return true; + } else { + // Pilot wants to take off, assume no groundcontact anymore and therefore allow thrust + _ground_contact_hysteresis.set_state_and_update(false); + } } - // If in manual flight mode never report landed if the user has more than idle throttle - // Check if user commands throttle and if so, report not landed based on - // the user intent to take off (even if the system might physically still have - // ground contact at this point). - if (manual_control_present && _manual.z > 0.15f) { - return false; + if (_get_minimal_thrust()) { + if (_min_trust_start == 0) { + _min_trust_start = now; + } + + } else { + _min_trust_start = 0; } // Return status based on armed state and throttle if no position lock is available. - if (_vehicleLocalPosition.timestamp == 0 || - hrt_elapsed_time(&_vehicleLocalPosition.timestamp) > 500000 || - !_vehicleLocalPosition.xy_valid || - !_vehicleLocalPosition.z_valid) { - + if (!_get_position_lock_available()) { // The system has minimum trust set (manual or in failsafe) // if this persists for 8 seconds AND the drone is not // falling consider it to be landed. This should even sustain @@ -218,28 +237,6 @@ bool MulticopterLandDetector::_get_ground_contact_state() armThresholdFactor = 2.5f; } - // Check if we are moving vertically - this might see a spike after arming due to - // throttle-up vibration. If accelerating fast the throttle thresholds will still give - // an accurate in-air indication. - bool verticalMovement = fabsf(_vehicleLocalPosition.vz) > _params.maxClimbRate * armThresholdFactor; - - if (!minimalThrust || verticalMovement) { - return false; - } - - return true; -} - -bool MulticopterLandDetector::_get_landed_state() -{ - float armThresholdFactor = 1.0f; - - // Widen acceptance thresholds for landed state right after arming - // so that motor spool-up and other effects do not trigger false negatives. - if (hrt_elapsed_time(&_arming_time) < LAND_DETECTOR_ARM_PHASE_TIME_US) { - armThresholdFactor = 2.5f; - } - // Check if we are moving horizontally. bool horizontalMovement = sqrtf(_vehicleLocalPosition.vx * _vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy) > _params.maxVelocity; @@ -251,16 +248,15 @@ bool MulticopterLandDetector::_get_landed_state() (fabsf(_vehicleAttitude.pitchspeed) > maxRotationScaled) || (fabsf(_vehicleAttitude.yawspeed) > maxRotationScaled); - - if (!_ground_contact_hysteresis.get_state() || rotating || horizontalMovement) { - // Sensed movement or thottle high, so reset the land detector. - return false; + if (_ground_contact_hysteresis.get_state() && _get_minimal_thrust() && !rotating && !horizontalMovement) { + // Ground contact, no thrust and no movement -> landed + return true; } - return true; + return false; } -float MulticopterLandDetector::get_takeoff_throttle() +float MulticopterLandDetector::_get_takeoff_throttle() { /* Position mode */ if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_position_enabled) { @@ -280,5 +276,32 @@ float MulticopterLandDetector::get_takeoff_throttle() return 0.0f; } +bool MulticopterLandDetector::_get_position_lock_available() +{ + return !(_vehicleLocalPosition.timestamp == 0 || + hrt_elapsed_time(&_vehicleLocalPosition.timestamp) > 500000 || + !_vehicleLocalPosition.xy_valid || + !_vehicleLocalPosition.z_valid); +} + +bool MulticopterLandDetector::_get_manual_control_present() +{ + return _control_mode.flag_control_manual_enabled && _manual.timestamp > 0; +} + +bool MulticopterLandDetector::_get_minimal_thrust() +{ + // 10% of throttle range between min and hover + float sys_min_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * _params.throttleRange; + + // Determine the system min throttle based on flight mode + if (!_control_mode.flag_control_altitude_enabled) { + sys_min_throttle = (_params.minManThrottle + 0.01f); + } + + // Check if thrust output is less than the minimum auto throttle param. + return _actuators.control[3] <= sys_min_throttle; +} + } diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 8513a6f49b..d3a6e77125 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -84,7 +84,7 @@ private: param_t maxVelocity; param_t maxRotation; param_t minThrottle; - param_t hoverThrottleAuto; + param_t hoverThrottle; param_t throttleRange; param_t minManThrottle; param_t freefall_acc_threshold; @@ -96,7 +96,7 @@ private: float maxVelocity; float maxRotation_rad_s; float minThrottle; - float hoverThrottleAuto; + float hoverThrottle; float throttleRange; float minManThrottle; float freefall_acc_threshold; @@ -123,7 +123,10 @@ private: uint64_t _arming_time; /* get control mode dependent pilot throttle threshold with which we should quit landed state and take off */ - float get_takeoff_throttle(); + float _get_takeoff_throttle(); + bool _get_position_lock_available(); + bool _get_manual_control_present(); + bool _get_minimal_thrust(); };