From 5c1d2c1cee79a88fdfca57faf2c18b7c33a98ea6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 29 Apr 2016 14:50:45 +0200 Subject: [PATCH] Land-detector: Better granularity for manual and auto flight modes --- src/modules/land_detector/LandDetector.cpp | 9 +++-- .../land_detector/MulticopterLandDetector.cpp | 39 ++++++++++++++----- .../land_detector/MulticopterLandDetector.h | 3 ++ 3 files changed, 37 insertions(+), 14 deletions(-) diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index f1ef61e625..5588f981f6 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -51,10 +51,11 @@ namespace landdetection LandDetector::LandDetector() : _landDetectedPub(0), _landDetected( {0, false}), - _arming_time(0), - _taskShouldExit(false), - _taskIsRunning(false), -_work{} { + _arming_time(0), + _taskShouldExit(false), + _taskIsRunning(false), + _work{} +{ // ctor } diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index cd4c0be147..b6cf0a31b5 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -57,11 +57,14 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), _parameterSub(-1), _attitudeSub(-1), _manualSub(-1), + _ctrl_state_sub(-1), + _vehicle_control_mode_sub(-1), _vehicleLocalPosition{}, _actuators{}, _arming{}, _vehicleAttitude{}, _ctrl_state{}, + _ctrl_mode{}, _landTimer(0), _freefallTimer(0) { @@ -79,11 +82,12 @@ void MulticopterLandDetector::initialize() // subscribe to position, attitude, arming and velocity changes _vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position)); _attitudeSub = orb_subscribe(ORB_ID(vehicle_attitude)); - _actuatorsSub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + _actuatorsSub = orb_subscribe(ORB_ID(actuator_controls_0)); _armingSub = orb_subscribe(ORB_ID(actuator_armed)); _parameterSub = orb_subscribe(ORB_ID(parameter_update)); _manualSub = orb_subscribe(ORB_ID(manual_control_setpoint)); _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); + _vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); // download parameters updateParameterCache(true); @@ -93,10 +97,11 @@ void MulticopterLandDetector::updateSubscriptions() { orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition); orb_update(ORB_ID(vehicle_attitude), _attitudeSub, &_vehicleAttitude); - orb_update(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _actuatorsSub, &_actuators); + orb_update(ORB_ID(actuator_controls_0), _actuatorsSub, &_actuators); orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); orb_update(ORB_ID(manual_control_setpoint), _manualSub, &_manual); orb_update(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); + orb_update(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub, &_ctrl_mode); } LandDetectionResult MulticopterLandDetector::update() @@ -148,8 +153,15 @@ 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)); + float sys_min_throttle = (_params.maxThrottle + 0.01f); + + // Determine the system min throttle based on flight mode + if (!_ctrl_mode.flag_control_altitude_enabled) { + sys_min_throttle = (_params.minManThrottle + 0.01f); + } + + // Check if thrust output is less than the minimum auto throttle param. + bool minimalThrust = (_actuators.control[3] <= sys_min_throttle); if (minimalThrust && _min_trust_start == 0) { _min_trust_start = now; @@ -166,19 +178,26 @@ bool MulticopterLandDetector::get_landed_state() _arming_time = now; } + // 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.timestamp > 0 && _manual.z > 0.15f && _ctrl_mode.flag_control_manual_enabled) { + return false; + } + // 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) { - // Check if user commands throttle and if so, report not landed - if (_manual.z > _params.minManThrottle + 0.01f) { - return false; - } - + // 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 + // quite acrobatic flight. if ((_min_trust_start > 0) && - (hrt_elapsed_time(&_min_trust_start) > 5 * 1000 * 1000)) { + (hrt_elapsed_time(&_min_trust_start) > 8 * 1000 * 1000)) { return !get_freefall_state(); } else { return false; diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 0a589849e4..20bb611cb7 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -51,6 +51,7 @@ #include #include #include +#include #include namespace landdetection @@ -125,6 +126,7 @@ private: int _attitudeSub; int _manualSub; int _ctrl_state_sub; + int _vehicle_control_mode_sub; struct vehicle_local_position_s _vehicleLocalPosition; struct actuator_controls_s _actuators; @@ -132,6 +134,7 @@ private: struct vehicle_attitude_s _vehicleAttitude; struct manual_control_setpoint_s _manual; 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