From a32a8966b72912c5e961d969d048b533bf6f1814 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 26 Mar 2018 08:55:39 -0400 Subject: [PATCH 1/2] sensors only publish airspeed if finite --- src/modules/sensors/sensors.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index bc1d6606caca..59ad5cec83df 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -372,8 +372,10 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) airspeed.air_temperature_celsius = air_temperature_celsius; - int instance; - orb_publish_auto(ORB_ID(airspeed), &_airspeed_pub, &airspeed, &instance, ORB_PRIO_DEFAULT); + if (PX4_ISFINITE(airspeed.indicated_airspeed_m_s) && PX4_ISFINITE(airspeed.true_airspeed_m_s)) { + int instance; + orb_publish_auto(ORB_ID(airspeed), &_airspeed_pub, &airspeed, &instance, ORB_PRIO_DEFAULT); + } } } From 1152e8792eec4b6b2822ac68bfb54500ef3c6acb Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 26 Mar 2018 08:55:55 -0400 Subject: [PATCH 2/2] 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 dca943345563..8ab1907abe8c 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; }