diff --git a/msg/vehicle_land_detected.msg b/msg/vehicle_land_detected.msg index ce6ea8e9a1..d886d31b05 100644 --- a/msg/vehicle_land_detected.msg +++ b/msg/vehicle_land_detected.msg @@ -1,2 +1,3 @@ uint64 timestamp # timestamp of the setpoint bool landed # true if vehicle is currently landed on the ground +bool freefall # true if vehicle is currently in free-fall diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 327e780a32..de41a1aa41 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -46,6 +46,9 @@ #include #include +namespace landdetection +{ + FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), _paramHandle(), _params(), @@ -84,11 +87,30 @@ void FixedwingLandDetector::updateSubscriptions() orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); } -bool FixedwingLandDetector::update() +LandDetectionResult FixedwingLandDetector::update() { // First poll for new data from our subscriptions updateSubscriptions(); + if (get_freefall_state()) { + _state = LANDDETECTION_RES_FREEFALL; + }else if(get_landed_state()){ + _state = LANDDETECTION_RES_LANDED; + }else{ + _state = LANDDETECTION_RES_FLYING; + } + + return _state; +} + +bool FixedwingLandDetector::get_freefall_state() +{ + //TODO + return false; +} + +bool FixedwingLandDetector::get_landed_state() +{ // only trigger flight conditions if we are armed if (!_arming.armed) { return true; @@ -159,3 +181,5 @@ void FixedwingLandDetector::updateParameterCache(const bool force) param_get(_paramHandle.maxIntVelocity, &_params.maxIntVelocity); } } + +} diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index c516788a3f..f450f94b82 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -49,6 +49,9 @@ #include #include +namespace landdetection +{ + class FixedwingLandDetector : public LandDetector { public: @@ -58,7 +61,7 @@ protected: /** * @brief blocking loop, should be run in a separate thread or task. Runs at 50Hz **/ - bool update() override; + LandDetectionResult update() override; /** * @brief Initializes the land detection algorithm @@ -70,6 +73,16 @@ protected: **/ void updateSubscriptions(); + /** + * @brief get UAV landed state + **/ + bool get_landed_state(); + + /** + * @brief returns true if UAV is in free-fall state + **/ + bool get_freefall_state(); + private: /** * @brief download and update local parameter cache @@ -109,4 +122,6 @@ private: uint64_t _landDetectTrigger; }; +} + #endif //__FIXED_WING_LAND_DETECTOR_H__ diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 5140a2809c..a8b61fb1ae 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -45,6 +45,9 @@ #include #include +namespace landdetection +{ + LandDetector::LandDetector() : _landDetectedPub(0), _landDetected( {0, false}), @@ -88,6 +91,7 @@ void LandDetector::cycle() // advertise the first land detected uORB _landDetected.timestamp = hrt_absolute_time(); _landDetected.landed = false; + _landDetected.freefall = false; _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); // initialize land detection algorithm @@ -98,12 +102,15 @@ void LandDetector::cycle() _taskShouldExit = false; } - bool landDetected = update(); + LandDetectionResult current_state = update(); + bool landDetected = (current_state==LANDDETECTION_RES_LANDED); + bool freefallDetected = (current_state==LANDDETECTION_RES_FREEFALL); // publish if land detection state has changed - if (_landDetected.landed != landDetected) { + if ((_landDetected.landed != landDetected) || (_landDetected.freefall != freefallDetected)) { _landDetected.timestamp = hrt_absolute_time(); _landDetected.landed = landDetected; + _landDetected.freefall = freefallDetected; // publish the land detected broadcast orb_publish(ORB_ID(vehicle_land_detected), (orb_advert_t)_landDetectedPub, &_landDetected); @@ -135,3 +142,5 @@ bool LandDetector::orb_update(const struct orb_metadata *meta, int handle, void return true; } + +} diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index 4b418f6523..ce7b862684 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -45,6 +45,15 @@ #include #include +namespace landdetection +{ + +enum LandDetectionResult { + LANDDETECTION_RES_FLYING = 0, /**< UAV is flying */ + LANDDETECTION_RES_LANDED = 1, /**< Land has been detected */ + LANDDETECTION_RES_FREEFALL = 2 /**< Free-fall has been detected */ +}; + class LandDetector { public: @@ -81,7 +90,7 @@ protected: * @brief Pure abstract method that must be overriden by sub-classes. This actually runs the underlying algorithm * @return true if a landing was detected and this should be broadcast to the rest of the system **/ - virtual bool update() = 0; + virtual LandDetectionResult update() = 0; /** * @brief Pure abstract method that is called by this class once for initializing the uderlying algorithm (memory allocation, @@ -106,6 +115,7 @@ protected: 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 */ + LandDetectionResult _state; /**< Result of land detection. Can be LANDDETECTION_RES_FLYING, LANDDETECTION_RES_LANDED or LANDDETECTION_RES_FREEFALL */ private: bool _taskShouldExit; /**< true if it is requested that this task should exit */ @@ -115,4 +125,6 @@ private: void cycle(); }; +} + #endif //__LAND_DETECTOR_H__ diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 650ae5724d..03ece61eb2 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -45,6 +45,9 @@ #include #include +namespace landdetection +{ + MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), _paramHandle(), _params(), @@ -89,14 +92,28 @@ void MulticopterLandDetector::updateSubscriptions() orb_update(ORB_ID(manual_control_setpoint), _manualSub, &_manual); } -bool MulticopterLandDetector::update() +LandDetectionResult MulticopterLandDetector::update() { // first poll for new data from our subscriptions updateSubscriptions(); updateParameterCache(false); - return get_landed_state(); + if (get_freefall_state()) { + _state = LANDDETECTION_RES_FREEFALL; + }else if(get_landed_state()){ + _state = LANDDETECTION_RES_LANDED; + }else{ + _state = LANDDETECTION_RES_FLYING; + } + + return _state; +} + +bool MulticopterLandDetector::get_freefall_state() +{ + //TODO + return false; } bool MulticopterLandDetector::get_landed_state() @@ -183,3 +200,5 @@ void MulticopterLandDetector::updateParameterCache(const bool force) param_get(_paramHandle.maxThrottle, &_params.maxThrottle); } } + +} diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 4e520fea94..7eda290d8b 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -52,6 +52,9 @@ #include #include +namespace landdetection +{ + class MulticopterLandDetector : public LandDetector { public: @@ -66,7 +69,7 @@ protected: /** * @brief Runs one iteration of the land detection algorithm **/ - virtual bool update() override; + virtual LandDetectionResult update() override; /** * @brief Initializes the land detection algorithm @@ -83,6 +86,11 @@ protected: **/ bool get_landed_state(); + /** + * @brief returns true if multicopter is in free-fall state + **/ + bool get_freefall_state(); + private: /** @@ -120,4 +128,6 @@ private: uint64_t _landTimer; }; +} + #endif //__MULTICOPTER_LAND_DETECTOR_H__ diff --git a/src/modules/land_detector/VtolLandDetector.cpp b/src/modules/land_detector/VtolLandDetector.cpp index 6c2fa9be8a..50a39c5fea 100644 --- a/src/modules/land_detector/VtolLandDetector.cpp +++ b/src/modules/land_detector/VtolLandDetector.cpp @@ -41,6 +41,9 @@ #include "VtolLandDetector.h" #include +namespace landdetection +{ + VtolLandDetector::VtolLandDetector() : MulticopterLandDetector(), _paramHandle(), _params(), @@ -69,7 +72,7 @@ void VtolLandDetector::updateSubscriptions() orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); } -bool VtolLandDetector::update() +LandDetectionResult VtolLandDetector::update() { updateSubscriptions(); updateParameterCache(false); @@ -94,7 +97,9 @@ bool VtolLandDetector::update() _was_in_air = !landed; - return landed; + _state = (landed) ? LANDDETECTION_RES_LANDED : LANDDETECTION_RES_FLYING; + + return _state; } void VtolLandDetector::updateParameterCache(const bool force) @@ -114,3 +119,5 @@ void VtolLandDetector::updateParameterCache(const bool force) param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed); } } + +} diff --git a/src/modules/land_detector/VtolLandDetector.h b/src/modules/land_detector/VtolLandDetector.h index 2c942ed06f..a9b409f610 100644 --- a/src/modules/land_detector/VtolLandDetector.h +++ b/src/modules/land_detector/VtolLandDetector.h @@ -44,6 +44,9 @@ #include "MulticopterLandDetector.h" #include +namespace landdetection +{ + class VtolLandDetector : public MulticopterLandDetector { public: @@ -59,7 +62,7 @@ private: /** * @brief Runs one iteration of the land detection algorithm **/ - bool update() override; + LandDetectionResult update() override; /** * @brief Initializes the land detection algorithm @@ -92,3 +95,5 @@ private: }; #endif + +} diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index 1179b10eae..5572c1b2f1 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -55,6 +55,9 @@ #include "MulticopterLandDetector.h" #include "VtolLandDetector.h" +namespace landdetection +{ + //Function prototypes static int land_detector_start(const char *mode); static void land_detector_stop(); @@ -208,3 +211,5 @@ exiterr: PX4_WARN("mode can either be 'fixedwing' or 'multicopter'"); return 1; } + +}