From 25300a6b11ae5d363b2eafe24718d44c454a859e Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Mon, 26 Mar 2018 08:55:55 -0400
Subject: [PATCH] VtolLandDetector add airspeed finite check
---
src/modules/land_detector/VtolLandDetector.cpp | 8 +++++---
1 file changed, 5 insertions(+), 3 deletions(-)
diff --git a/src/modules/land_detector/VtolLandDetector.cpp b/src/modules/land_detector/VtolLandDetector.cpp
index dca9433455..8ab1907abe 100644
--- a/src/modules/land_detector/VtolLandDetector.cpp
+++ b/src/modules/land_detector/VtolLandDetector.cpp
@@ -88,8 +88,10 @@ bool VtolLandDetector::_get_landed_state()
bool landed = MulticopterLandDetector::_get_landed_state();
// for vtol we additionally consider airspeed
- if (hrt_elapsed_time(&_airspeed.timestamp) < 500 * 1000) {
- _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s;
+ if (hrt_elapsed_time(&_airspeed.timestamp) < 500 * 1000 && _airspeed.confidence > 0.99f
+ && PX4_ISFINITE(_airspeed.indicated_airspeed_m_s)) {
+
+ _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.indicated_airspeed_m_s;
} else {
// if airspeed does not update, set it to zero and rely on multicopter land detector
@@ -98,7 +100,7 @@ bool VtolLandDetector::_get_landed_state()
// only consider airspeed if we have been in air before to avoid false
// detections in the case of wind on the ground
- if (_was_in_air && _airspeed_filtered > _params.maxAirSpeed) {
+ if (_was_in_air && (_airspeed_filtered > _params.maxAirSpeed)) {
landed = false;
}
--
GitLab