From 3ccd9988d3736c71f5f6c7414a6a8a281bc256bb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 29 Apr 2016 12:34:58 +0200 Subject: [PATCH] Multicopter land detector: Enforce sync between system and detector --- src/modules/land_detector/LandDetector.cpp | 2 +- .../land_detector/MulticopterLandDetector.cpp | 41 ++++++++++++------- .../land_detector/MulticopterLandDetector.h | 9 ++-- .../land_detector/land_detector_params.c | 16 +------- 4 files changed, 35 insertions(+), 33 deletions(-) diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 04d5f12f60..f1ef61e625 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2016 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index a7367f934c..cd4c0be147 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -68,7 +68,8 @@ 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("LNDMC_THR_MAX"); + _paramHandle.maxThrottle = param_find("MPC_THR_MIN"); + _paramHandle.minManThrottle = param_find("MPC_MANTHR_MIN"); _paramHandle.acc_threshold_m_s2 = param_find("LNDMC_FFALL_THR"); _paramHandle.ff_trigger_time = param_find("LNDMC_FFALL_TTRI"); } @@ -144,35 +145,46 @@ bool MulticopterLandDetector::get_freefall_state() bool MulticopterLandDetector::get_landed_state() { + // Time base for this function + const uint64_t now = hrt_absolute_time(); + + // Check if thrust output is less than max throttle param. + bool minimalThrust = (_actuators.control[3] <= (_params.maxThrottle + 0.05f)); + + if (minimalThrust && _min_trust_start == 0) { + _min_trust_start = now; + } else if (!minimalThrust) { + _min_trust_start = 0; + } + // only trigger flight conditions if we are armed if (!_arming.armed) { _arming_time = 0; return true; } else if (_arming_time == 0) { - _arming_time = hrt_absolute_time(); + _arming_time = now; } - // Check if user commands throttle and if so, report not landed - if (_manual.z > 0.3f) { - return false; - } - - // Check if thrust output is less than max throttle param. - bool minimalThrust = _actuators.control[3] <= _params.maxThrottle; - // 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) { - // Minimal thrust means landed. - return minimalThrust; + // Check if user commands throttle and if so, report not landed + if (_manual.z > _params.minManThrottle + 0.01f) { + return false; + } + + if ((_min_trust_start > 0) && + (hrt_elapsed_time(&_min_trust_start) > 5 * 1000 * 1000)) { + return !get_freefall_state(); + } else { + return false; + } } - const uint64_t now = hrt_absolute_time(); - float armThresholdFactor = 1.0f; // Widen acceptance thresholds for landed state right after arming @@ -224,6 +236,7 @@ void MulticopterLandDetector::updateParameterCache(const bool force) 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.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); } diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index b842b0b956..0a589849e4 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -102,6 +102,7 @@ private: param_t maxVelocity; param_t maxRotation; param_t maxThrottle; + param_t minManThrottle; param_t acc_threshold_m_s2; param_t ff_trigger_time; } _paramHandle; @@ -111,6 +112,7 @@ private: float maxVelocity; float maxRotation_rad_s; float maxThrottle; + float minManThrottle; float acc_threshold_m_s2; float ff_trigger_time; } _params; @@ -129,10 +131,11 @@ private: struct actuator_armed_s _arming; struct vehicle_attitude_s _vehicleAttitude; struct manual_control_setpoint_s _manual; - struct control_state_s _ctrl_state; + struct control_state_s _ctrl_state; - 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 _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 }; } diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c index 99f55f5f4c..faba2c7981 100644 --- a/src/modules/land_detector/land_detector_params.c +++ b/src/modules/land_detector/land_detector_params.c @@ -74,20 +74,6 @@ PARAM_DEFINE_FLOAT(LNDMC_XY_VEL_MAX, 1.50f); */ PARAM_DEFINE_FLOAT(LNDMC_ROT_MAX, 20.0f); -/** - * Multicopter max throttle - * - * Maximum actuator output on throttle allowed in the landed state - * - * @unit norm - * @min 0.1 - * @max 0.5 - * @decimal 2 - * - * @group Land Detector - */ -PARAM_DEFINE_FLOAT(LNDMC_THR_MAX, 0.15f); - /** * Multicopter specific force threshold * @@ -105,7 +91,7 @@ PARAM_DEFINE_FLOAT(LNDMC_FFALL_THR, 2.0f); /** * Multicopter free-fall trigger time * - * Milliseconds that freefall conditions have to hold before triggering a freefall. + * Seconds (decimal) that freefall conditions have to met before triggering a freefall. * Minimal value is limited by LAND_DETECTOR_UPDATE_RATE=50Hz in landDetector.h * * @unit s