From 7bb952baeddcb83ac0238955dd805624ef07d7e6 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Sat, 28 Dec 2019 18:32:41 +0100 Subject: [PATCH] update airspeed usage to airspeed_validated (#13710) * 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. * 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. * 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 --- .../land_detector/FixedwingLandDetector.cpp | 10 ++++++++-- src/modules/land_detector/FixedwingLandDetector.h | 6 +++--- src/modules/mavlink/mavlink_messages.cpp | 14 +++++++------- 3 files changed, 18 insertions(+), 12 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 5da46baf00..c7ce8028b3 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,13 @@ 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 + 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. diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index b8ca1fd701..19bff21757 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 @@ private: 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}; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index ef0c387056..ffec72487b 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 @@ private: 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 @@ protected: _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 @@ protected: { 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));