From cb95c9c5e212b4b580d5c675e0edd7e616fc7f7c Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 9 Dec 2019 18:49:49 +0100 Subject: [PATCH 1/3] Mavlink: subscribe to airspeed_validated instead of airspeed topic This e.g. changes the way QGC displays the airspeed in case of an airspeed failure (0 instead of the last valid airspeed). It will always display the airspeed that's used currently in the control modules. Signed-off-by: Silvan Fuhrer --- src/modules/mavlink/mavlink_messages.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index ef0c38705661..ffec72487b57 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -58,7 +58,7 @@ #include #include #include -#include +#include #include #include #include @@ -1398,7 +1398,7 @@ class MavlinkStreamVFRHUD : public MavlinkStream MavlinkOrbSubscription *_act0_sub; MavlinkOrbSubscription *_act1_sub; - MavlinkOrbSubscription *_airspeed_sub; + MavlinkOrbSubscription *_airspeed_validated_sub; uint64_t _airspeed_time; MavlinkOrbSubscription *_air_data_sub; @@ -1415,7 +1415,7 @@ class MavlinkStreamVFRHUD : public MavlinkStream _armed_time(0), _act0_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))), _act1_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_1))), - _airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))), + _airspeed_validated_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed_validated))), _airspeed_time(0), _air_data_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_air_data))) {} @@ -1424,16 +1424,16 @@ class MavlinkStreamVFRHUD : public MavlinkStream { vehicle_local_position_s pos = {}; actuator_armed_s armed = {}; - airspeed_s airspeed = {}; + airspeed_validated_s airspeed_validated = {}; bool updated = false; updated |= _pos_sub->update(&_pos_time, &pos); updated |= _armed_sub->update(&_armed_time, &armed); - updated |= _airspeed_sub->update(&_airspeed_time, &airspeed); + updated |= _airspeed_validated_sub->update(&_airspeed_time, &airspeed_validated); if (updated) { - mavlink_vfr_hud_t msg = {}; - msg.airspeed = airspeed.indicated_airspeed_m_s; + mavlink_vfr_hud_t msg{}; + msg.airspeed = airspeed_validated.indicated_airspeed_m_s; msg.groundspeed = sqrtf(pos.vx * pos.vx + pos.vy * pos.vy); msg.heading = math::degrees(wrap_2pi(pos.yaw)); From f2a2f1f73ebb2ed13b5a5cfb84d7f21c8cbb5dd2 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 9 Dec 2019 18:52:17 +0100 Subject: [PATCH 2/3] FW land detector: move to subscribe to airspeed_validated instead of airspeed topic - the land detector checks further if the airspeed is NAN, in which case it sets the airspeed to 0 (min groundspeed, vz and accel checks still have to pass. Signed-off-by: Silvan Fuhrer --- src/modules/land_detector/FixedwingLandDetector.cpp | 6 ++++-- src/modules/land_detector/FixedwingLandDetector.h | 6 +++--- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 5da46baf00e2..2d315a677310 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -54,7 +54,7 @@ FixedwingLandDetector::FixedwingLandDetector() void FixedwingLandDetector::_update_topics() { LandDetector::_update_topics(); - _airspeed_sub.update(&_airspeed); + _airspeed_validated_sub.update(&_airspeed_validated); } bool FixedwingLandDetector::_get_landed_state() @@ -83,7 +83,9 @@ bool FixedwingLandDetector::_get_landed_state() _velocity_z_filtered = val; } - _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; + // set _airspeed_filtered to 0 if airspeed data is invalid + _airspeed_filtered = PX4_ISFINITE(_airspeed_validated.true_airspeed_m_s) ? 0.95f * _airspeed_filtered + 0.05f * + _airspeed_validated.true_airspeed_m_s : 0.0f; // A leaking lowpass prevents biases from building up, but // gives a mostly correct response for short impulses. diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index b8ca1fd70170..19bff21757fa 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -43,7 +43,7 @@ #pragma once #include -#include +#include #include "LandDetector.h" @@ -69,9 +69,9 @@ class FixedwingLandDetector final : public LandDetector static constexpr hrt_abstime LANDED_TRIGGER_TIME_US = 2_s; static constexpr hrt_abstime FLYING_TRIGGER_TIME_US = 0_us; - uORB::Subscription _airspeed_sub{ORB_ID(airspeed)}; + uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)}; - airspeed_s _airspeed{}; + airspeed_validated_s _airspeed_validated{}; float _airspeed_filtered{0.0f}; float _velocity_xy_filtered{0.0f}; From e0d5e06dffdee2671470d8115dd80dcf96c01e43 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Sun, 15 Dec 2019 13:24:04 +0100 Subject: [PATCH 3/3] Fixed-wing land detector use airspeed_vaidated: addressed review comments - replaced ternary by conditional - set airspeed to 0 if airspeed_validated stops publishing Signed-off-by: Silvan Fuhrer --- src/modules/land_detector/FixedwingLandDetector.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 2d315a677310..c7ce8028b377 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -84,8 +84,12 @@ bool FixedwingLandDetector::_get_landed_state() } // set _airspeed_filtered to 0 if airspeed data is invalid - _airspeed_filtered = PX4_ISFINITE(_airspeed_validated.true_airspeed_m_s) ? 0.95f * _airspeed_filtered + 0.05f * - _airspeed_validated.true_airspeed_m_s : 0.0f; + if (!PX4_ISFINITE(_airspeed_validated.true_airspeed_m_s) || hrt_elapsed_time(&_airspeed_validated.timestamp) > 1_s) { + _airspeed_filtered = 0.0f; + + } else { + _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed_validated.true_airspeed_m_s; + } // A leaking lowpass prevents biases from building up, but // gives a mostly correct response for short impulses.