From fc9d31c67bc746d6bd659a0696532bf873cc8638 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 21 Jan 2020 16:28:18 +0100 Subject: [PATCH 1/2] VTOL Land Detector: move to airspeed_validated Signed-off-by: Silvan Fuhrer --- src/modules/land_detector/VtolLandDetector.cpp | 7 +++---- src/modules/land_detector/VtolLandDetector.h | 6 +++--- 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/src/modules/land_detector/VtolLandDetector.cpp b/src/modules/land_detector/VtolLandDetector.cpp index a3f4292e5319..6729d9864754 100644 --- a/src/modules/land_detector/VtolLandDetector.cpp +++ b/src/modules/land_detector/VtolLandDetector.cpp @@ -49,7 +49,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 +74,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 diff --git a/src/modules/land_detector/VtolLandDetector.h b/src/modules/land_detector/VtolLandDetector.h index 10f24f805932..681f6edcb8be 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" @@ -62,10 +62,10 @@ class VtolLandDetector : public MulticopterLandDetector 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 */ From 4b5b98d16efe53ea63d9b1413dd71d07b1f95ebf Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 21 Jan 2020 16:46:17 +0100 Subject: [PATCH 2/2] VTOL land detector: extend fall_detection from multicopter land detector and disable in fixed-wing mode Extend the get_freefall_state() from MC land detector to have a VTOL-specific one that just enables free fall detection if in rotary wing or transition mode. This is done to prevent wrong free-fall detected warnings while doing low-G maneuvers (parabolic flights). Land detection is anyway disabled in FW flight for VTOL so no logic change. Signed-off-by: Silvan Fuhrer --- src/modules/land_detector/VtolLandDetector.cpp | 10 ++++++++++ src/modules/land_detector/VtolLandDetector.h | 1 + 2 files changed, 11 insertions(+) diff --git a/src/modules/land_detector/VtolLandDetector.cpp b/src/modules/land_detector/VtolLandDetector.cpp index 6729d9864754..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" @@ -94,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 681f6edcb8be..d159728636e7 100644 --- a/src/modules/land_detector/VtolLandDetector.h +++ b/src/modules/land_detector/VtolLandDetector.h @@ -59,6 +59,7 @@ 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: