From 9f322a395e6abf0624dfd7cd6dc67e1e7fe93b90 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 19 Jul 2015 18:30:52 +0200 Subject: [PATCH] Make land detector more robust during initial spool-up --- .../land_detector/FixedwingLandDetector.cpp | 15 +++++++- .../land_detector/FixedwingLandDetector.h | 16 +++++--- src/modules/land_detector/LandDetector.cpp | 5 ++- src/modules/land_detector/LandDetector.h | 7 ++-- .../land_detector/MulticopterLandDetector.cpp | 37 +++++++++++++------ .../land_detector/MulticopterLandDetector.h | 14 +++---- 6 files changed, 65 insertions(+), 29 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 741bc02ad4..dad71be4ae 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -49,7 +49,11 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), _vehicleLocalPositionSub(-1), _vehicleLocalPosition({}), _airspeedSub(-1), - _airspeed({}), + _vehicleStatusSub(-1), + _armingSub(-1), + _airspeed{}, + _vehicleStatus{}, + _arming{}, _parameterSub(-1), _velocity_xy_filtered(0.0f), _velocity_z_filtered(0.0f), @@ -66,6 +70,8 @@ void FixedwingLandDetector::initialize() // Subscribe to local position and airspeed data _vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position)); _airspeedSub = orb_subscribe(ORB_ID(airspeed)); + _vehicleStatusSub = orb_subscribe(ORB_ID(vehicle_status)); + _armingSub = orb_subscribe(ORB_ID(actuator_armed)); updateParameterCache(true); } @@ -74,6 +80,8 @@ void FixedwingLandDetector::updateSubscriptions() { orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition); orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); + orb_update(ORB_ID(vehicle_status), _vehicleStatusSub, &_vehicleStatus); + orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); } bool FixedwingLandDetector::update() @@ -81,6 +89,11 @@ bool FixedwingLandDetector::update() // First poll for new data from our subscriptions updateSubscriptions(); + // only trigger flight conditions if we are armed + if (!_arming.armed) { + return true; + } + const uint64_t now = hrt_absolute_time(); bool landDetected = false; diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index 0e9c092ee0..325faee794 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -44,7 +44,9 @@ #include "LandDetector.h" #include #include +#include #include +#include #include class FixedwingLandDetector : public LandDetector @@ -90,11 +92,15 @@ private: } _params; private: - int _vehicleLocalPositionSub; /**< notification of local position */ - struct vehicle_local_position_s _vehicleLocalPosition; /**< the result from local position subscription */ - int _airspeedSub; - struct airspeed_s _airspeed; - int _parameterSub; + int _vehicleLocalPositionSub; /**< notification of local position */ + struct vehicle_local_position_s _vehicleLocalPosition; /**< the result from local position subscription */ + int _airspeedSub; + int _vehicleStatusSub; + int _armingSub; + struct airspeed_s _airspeed; + struct vehicle_status_s _vehicleStatus; + struct actuator_armed_s _arming; + int _parameterSub; float _velocity_xy_filtered; float _velocity_z_filtered; diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 7c830fc072..ba6211de64 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -46,8 +46,9 @@ LandDetector::LandDetector() : _landDetectedPub(-1), _landDetected({0, false}), - _taskShouldExit(false), - _taskIsRunning(false) + _arming_time(0), + _taskShouldExit(false), + _taskIsRunning(false) { // ctor } diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index 68f96288cd..20d124fdfa 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -96,10 +96,11 @@ protected: static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold before triggering a land */ + static constexpr uint64_t LAND_DETECTOR_ARM_PHASE_TIME = 1000000; /**< time interval in which wider acceptance thresholds are used after arming */ -protected: - orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ - struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ + orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ + struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ + uint64_t _arming_time; /**< timestamp of arming time */ private: bool _taskShouldExit; /**< true if it is requested that this task should exit */ diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 5391f7769a..1d0a6b92dc 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -53,12 +53,12 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), _actuatorsSub(-1), _armingSub(-1), _parameterSub(-1), - _attitudeSub(-1), - _vehicleGlobalPosition({}), - _vehicleStatus({}), - _actuators({}), - _arming({}), - _vehicleAttitude({}), + _attitudeSub(-1), + _vehicleGlobalPosition{}, + _vehicleStatus{}, + _actuators{}, + _arming{}, + _vehicleAttitude{}, _landTimer(0) { _paramHandle.maxRotation = param_find("LNDMC_ROT_MAX"); @@ -97,7 +97,10 @@ bool MulticopterLandDetector::update() // 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(); } // return status based on armed state if no position lock is available @@ -110,8 +113,18 @@ bool MulticopterLandDetector::update() const uint64_t now = hrt_absolute_time(); - // check if we are moving vertically - bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > _params.maxClimbRate; + 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) { + 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(_vehicleGlobalPosition.vel_d) > _params.maxClimbRate * armThresholdFactor; // check if we are moving horizontally bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n @@ -119,9 +132,11 @@ bool MulticopterLandDetector::update() && _vehicleStatus.condition_global_position_valid; // next look if all rotation angles are not moving - bool rotating = fabsf(_vehicleAttitude.rollspeed) > _params.maxRotation || - fabsf(_vehicleAttitude.pitchspeed) > _params.maxRotation || - fabsf(_vehicleAttitude.yawspeed) > _params.maxRotation; + float maxRotationScaled = _params.maxRotation * armThresholdFactor; + + bool rotating = (fabsf(_vehicleAttitude.rollspeed) > maxRotationScaled) || + (fabsf(_vehicleAttitude.pitchspeed) > maxRotationScaled) || + (fabsf(_vehicleAttitude.yawspeed) > maxRotationScaled); // check if thrust output is minimal (about half of default) bool minimalThrust = _actuators.control[3] <= _params.maxThrottle; diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 8c57416b56..d001be4e7f 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -97,20 +97,20 @@ private: } _params; private: - int _vehicleGlobalPositionSub; /**< notification of global position */ + int _vehicleGlobalPositionSub; /**< notification of global position */ int _vehicleStatusSub; int _actuatorsSub; int _armingSub; int _parameterSub; int _attitudeSub; - struct vehicle_global_position_s _vehicleGlobalPosition; /**< the result from global position subscription */ - struct vehicle_status_s _vehicleStatus; - struct actuator_controls_s _actuators; - struct actuator_armed_s _arming; - struct vehicle_attitude_s _vehicleAttitude; + struct vehicle_global_position_s _vehicleGlobalPosition; /**< the result from global position subscription */ + struct vehicle_status_s _vehicleStatus; + struct actuator_controls_s _actuators; + struct actuator_armed_s _arming; + struct vehicle_attitude_s _vehicleAttitude; - uint64_t _landTimer; /**< timestamp in microseconds since a possible land was detected*/ + uint64_t _landTimer; /**< timestamp in microseconds since a possible land was detected*/ }; #endif //__MULTICOPTER_LAND_DETECTOR_H__