diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 5da46baf00e2..c7ce8028b377 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 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}; 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));