diff --git a/src/modules/land_detector/VtolLandDetector.cpp b/src/modules/land_detector/VtolLandDetector.cpp index a3f4292e5319..1f6e962acf44 100644 --- a/src/modules/land_detector/VtolLandDetector.cpp +++ b/src/modules/land_detector/VtolLandDetector.cpp @@ -40,6 +40,7 @@ */ #include +#include #include "VtolLandDetector.h" @@ -49,7 +50,7 @@ namespace land_detector void VtolLandDetector::_update_topics() { MulticopterLandDetector::_update_topics(); - _airspeed_sub.update(&_airspeed); + _airspeed_validated_sub.update(&_airspeed_validated); _vehicle_status_sub.update(&_vehicle_status); } @@ -74,10 +75,9 @@ bool VtolLandDetector::_get_landed_state() bool landed = MulticopterLandDetector::_get_landed_state(); // for vtol we additionally consider airspeed - if (hrt_elapsed_time(&_airspeed.timestamp) < 1_s && _airspeed.confidence > 0.99f - && PX4_ISFINITE(_airspeed.indicated_airspeed_m_s)) { + if (hrt_elapsed_time(&_airspeed_validated.timestamp) < 1_s && PX4_ISFINITE(_airspeed_validated.true_airspeed_m_s)) { - _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.indicated_airspeed_m_s; + _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed_validated.true_airspeed_m_s; } else { // if airspeed does not update, set it to zero and rely on multicopter land detector @@ -95,4 +95,13 @@ bool VtolLandDetector::_get_landed_state() return landed; } +bool VtolLandDetector::_get_freefall_state() +{ + bool free_fall_detected = + MulticopterLandDetector::_get_freefall_state(); // true if falling or in a parabolic flight (low gravity) + + // only return a positive free fall detected if not in fixed-wing mode + return _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_FIXED_WING && free_fall_detected; +} + } // namespace land_detector diff --git a/src/modules/land_detector/VtolLandDetector.h b/src/modules/land_detector/VtolLandDetector.h index 10f24f805932..d159728636e7 100644 --- a/src/modules/land_detector/VtolLandDetector.h +++ b/src/modules/land_detector/VtolLandDetector.h @@ -41,7 +41,7 @@ #pragma once -#include +#include #include #include "MulticopterLandDetector.h" @@ -59,13 +59,14 @@ class VtolLandDetector : public MulticopterLandDetector void _update_topics() override; bool _get_landed_state() override; bool _get_maybe_landed_state() override; + bool _get_freefall_state() override; private: - uORB::Subscription _airspeed_sub{ORB_ID(airspeed)}; + uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; - airspeed_s _airspeed{}; + airspeed_validated_s _airspeed_validated{}; vehicle_status_s _vehicle_status{}; bool _was_in_air{false}; /**< indicates whether the vehicle was in the air in the previous iteration */