diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index 1b71e8d475..cfe5c87179 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2017 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 @@ -31,12 +31,13 @@ * ****************************************************************************/ -/* +/** * @file LandDetector.h Land detector interface for multicopter, fixedwing and VTOL implementations. * * @author Johan Jansen * @author Julian Oes + * @author Lorenz Meier */ #pragma once @@ -56,13 +57,14 @@ public: enum class LandDetectionState { FLYING = 0, LANDED = 1, - FREEFALL = 2 + FREEFALL = 2, + GROUND_CONTACT = 3 }; LandDetector(); virtual ~LandDetector(); - /* + /** * @return true if this task is currently running. */ inline bool is_running() const @@ -71,7 +73,7 @@ public: } - /* + /** * @return current state. */ LandDetectionState get_state() const @@ -79,57 +81,62 @@ public: return _state; } - /* + /** * Tells the task that it should exit. */ void stop(); - /* + /** * Get the work queue going. */ int start(); protected: - /* + /** * Called once to initialize uORB topics. */ virtual void _initialize_topics() = 0; - /* + /** * Update uORB topics. */ virtual void _update_topics() = 0; - /* + /** * Update parameters. */ virtual void _update_params() = 0; - /* + /** * @return true if UAV is in a landed state. */ virtual bool _get_landed_state() = 0; - /* + /** + * @return true if UAV is touching ground but not landed + */ + bool _get_ground_contact_state() { return _get_landed_state(); } + + /** * @return true if UAV is in free-fall state. */ virtual bool _get_freefall_state() = 0; - /* + /** * Convenience function for polling uORB subscriptions. * * @return true if there was new data and it was successfully copied */ static bool _orb_update(const struct orb_metadata *meta, int handle, void *buffer); - /* Run main land detector loop at this rate in Hz. */ + /** Run main land detector loop at this rate in Hz. */ static constexpr uint32_t LAND_DETECTOR_UPDATE_RATE_HZ = 50; - /* Time in us that landing conditions have to hold before triggering a land. */ + /** Time in us that landing conditions have to hold before triggering a land. */ static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME_US = 2000000; - /* Time interval in us in which wider acceptance thresholds are used after arming. */ + /** Time interval in us in which wider acceptance thresholds are used after arming. */ static constexpr uint64_t LAND_DETECTOR_ARM_PHASE_TIME_US = 2000000; orb_advert_t _landDetectedPub; diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 2f9fb4d9d2..dde19c3998 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -136,7 +136,7 @@ bool MulticopterLandDetector::_get_freefall_state() return (acc_norm < _params.freefall_acc_threshold); //true if we are currently falling } -bool MulticopterLandDetector::_get_landed_state() +bool MulticopterLandDetector::_get_ground_contact_state() { // Time base for this function const uint64_t now = hrt_absolute_time(); @@ -217,6 +217,23 @@ bool MulticopterLandDetector::_get_landed_state() // 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; @@ -229,7 +246,7 @@ bool MulticopterLandDetector::_get_landed_state() (fabsf(_vehicleAttitude.yawspeed) > maxRotationScaled); - if (verticalMovement || rotating || !minimalThrust || horizontalMovement) { + if (!_get_ground_contact_state() || rotating || horizontalMovement) { // Sensed movement or thottle high, so reset the land detector. return false; } diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 080194c3e6..a1e3e1e4f8 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -71,6 +71,8 @@ protected: virtual bool _get_landed_state() override; + bool _get_ground_contact_state(); + virtual bool _get_freefall_state() override; private: diff --git a/src/modules/land_detector/VtolLandDetector.cpp b/src/modules/land_detector/VtolLandDetector.cpp index de058d812f..de6b983b57 100644 --- a/src/modules/land_detector/VtolLandDetector.cpp +++ b/src/modules/land_detector/VtolLandDetector.cpp @@ -71,6 +71,11 @@ void VtolLandDetector::_update_topics() _orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); } +bool VtolLandDetector::_get_ground_contact_state() +{ + return MulticopterLandDetector::_get_ground_contact_state(); +} + bool VtolLandDetector::_get_landed_state() { // this is returned from the mutlicopter land detector diff --git a/src/modules/land_detector/VtolLandDetector.h b/src/modules/land_detector/VtolLandDetector.h index 31a1e448fb..4a0dd4faf0 100644 --- a/src/modules/land_detector/VtolLandDetector.h +++ b/src/modules/land_detector/VtolLandDetector.h @@ -62,6 +62,8 @@ protected: virtual bool _get_landed_state() override; + bool _get_ground_contact_state(); + virtual bool _get_freefall_state() override; private: