From 808dedf441de219d283c53464e0828c76adbd34e Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 21 Dec 2016 17:49:37 +0100 Subject: [PATCH] land detector: small refactor while studying --- src/modules/land_detector/LandDetector.h | 4 ++-- .../land_detector/MulticopterLandDetector.cpp | 21 ++++++++++--------- .../land_detector/MulticopterLandDetector.h | 8 +++---- 3 files changed, 17 insertions(+), 16 deletions(-) diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index 1b71e8d475..a2c321ac64 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -127,10 +127,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 = 2e6; /* Time interval in us in which wider acceptance thresholds are used after arming. */ - static constexpr uint64_t LAND_DETECTOR_ARM_PHASE_TIME_US = 2000000; + static constexpr uint64_t LAND_DETECTOR_ARM_PHASE_TIME_US = 2e6; orb_advert_t _landDetectedPub; struct vehicle_land_detected_s _landDetected; diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 7bd347b422..0f4fa939bf 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -72,9 +72,9 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), _paramHandle.maxRotation = param_find("LNDMC_ROT_MAX"); _paramHandle.maxVelocity = param_find("LNDMC_XY_VEL_MAX"); _paramHandle.maxClimbRate = param_find("LNDMC_Z_VEL_MAX"); - _paramHandle.maxThrottle = param_find("MPC_THR_MIN"); + _paramHandle.minThrottle = param_find("MPC_THR_MIN"); _paramHandle.minManThrottle = param_find("MPC_MANTHR_MIN"); - _paramHandle.acc_threshold_m_s2 = param_find("LNDMC_FFALL_THR"); + _paramHandle.ff_acc_threshold = param_find("LNDMC_FFALL_THR"); _paramHandle.ff_trigger_time = param_find("LNDMC_FFALL_TTRI"); } @@ -108,9 +108,9 @@ void MulticopterLandDetector::_update_params() param_get(_paramHandle.maxVelocity, &_params.maxVelocity); param_get(_paramHandle.maxRotation, &_params.maxRotation_rad_s); _params.maxRotation_rad_s = math::radians(_params.maxRotation_rad_s); - param_get(_paramHandle.maxThrottle, &_params.maxThrottle); + param_get(_paramHandle.minThrottle, &_params.minThrottle); param_get(_paramHandle.minManThrottle, &_params.minManThrottle); - param_get(_paramHandle.acc_threshold_m_s2, &_params.acc_threshold_m_s2); + param_get(_paramHandle.ff_acc_threshold, &_params.ff_acc_threshold); param_get(_paramHandle.ff_trigger_time, &_params.ff_trigger_time); _freefall_hysteresis.set_hysteresis_time_from(false, 1e6f * _params.ff_trigger_time); } @@ -118,8 +118,8 @@ void MulticopterLandDetector::_update_params() bool MulticopterLandDetector::_get_freefall_state() { - if (_params.acc_threshold_m_s2 < 0.1f - || _params.acc_threshold_m_s2 > 10.0f) { //if parameter is set to zero or invalid, disable free-fall detection. + if (_params.ff_acc_threshold < 0.1f + || _params.ff_acc_threshold > 10.0f) { //if parameter is set to zero or invalid, disable free-fall detection. return false; } @@ -133,7 +133,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. - return (acc_norm < _params.acc_threshold_m_s2); //true if we are currently falling + return (acc_norm < _params.ff_acc_threshold); //true if we are currently falling } bool MulticopterLandDetector::_get_landed_state() @@ -141,7 +141,7 @@ bool MulticopterLandDetector::_get_landed_state() // Time base for this function const uint64_t now = hrt_absolute_time(); - float sys_min_throttle = (_params.maxThrottle + 0.01f); + float sys_min_throttle = (_params.minThrottle + 0.01f); // Determine the system min throttle based on flight mode if (!_ctrl_mode.flag_control_altitude_enabled) { @@ -154,6 +154,7 @@ bool MulticopterLandDetector::_get_landed_state() if (minimalThrust && _min_trust_start == 0) { _min_trust_start = now; + } else if (!minimalThrust) { _min_trust_start = 0; } @@ -178,7 +179,7 @@ bool MulticopterLandDetector::_get_landed_state() // Return status based on armed state and throttle if no position lock is available. if (_vehicleLocalPosition.timestamp == 0 || - hrt_elapsed_time(&_vehicleLocalPosition.timestamp) > 500000 || + hrt_elapsed_time(&_vehicleLocalPosition.timestamp) > 0.5e6 || !_vehicleLocalPosition.xy_valid || !_vehicleLocalPosition.z_valid) { @@ -187,7 +188,7 @@ bool MulticopterLandDetector::_get_landed_state() // falling consider it to be landed. This should even sustain // quite acrobatic flight. if ((_min_trust_start > 0) && - (hrt_elapsed_time(&_min_trust_start) > 8 * 1000 * 1000)) { + (hrt_elapsed_time(&_min_trust_start) > 8e6)) { return true; diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index dbfaf9aa75..f3e29b2c7e 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -81,9 +81,9 @@ private: param_t maxClimbRate; param_t maxVelocity; param_t maxRotation; - param_t maxThrottle; + param_t minThrottle; param_t minManThrottle; - param_t acc_threshold_m_s2; + param_t ff_acc_threshold; param_t ff_trigger_time; } _paramHandle; @@ -91,9 +91,9 @@ private: float maxClimbRate; float maxVelocity; float maxRotation_rad_s; - float maxThrottle; + float minThrottle; float minManThrottle; - float acc_threshold_m_s2; + float ff_acc_threshold; float ff_trigger_time; } _params;