From f7bb5d13f7a6fea7f7683eb75b0b40e299b80075 Mon Sep 17 00:00:00 2001 From: Silvan Date: Fri, 4 Apr 2025 16:56:40 +0200 Subject: [PATCH] At every consumation of AirspeedValidated, check for SOURCE Signed-off-by: Silvan --- .../HealthAndArmingChecks/checks/airspeedCheck.cpp | 10 ++++++++-- .../fw_pos_control/FixedwingPositionControl.cpp | 4 +++- src/modules/land_detector/FixedwingLandDetector.cpp | 9 +++++++-- src/modules/mavlink/streams/VFR_HUD.hpp | 6 +++++- 4 files changed, 23 insertions(+), 6 deletions(-) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/airspeedCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/airspeedCheck.cpp index a9af3ff054..0e15e856ca 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/airspeedCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/airspeedCheck.cpp @@ -54,6 +54,12 @@ void AirspeedChecks::checkAndReport(const Context &context, Report &reporter) reporter.setIsPresent(health_component_t::differential_pressure); + const bool airspeed_from_sensor = airspeed_validated.airspeed_source == airspeed_validated_s::SENSOR_1 + || airspeed_validated.airspeed_source == airspeed_validated_s::SENSOR_2 + || airspeed_validated.airspeed_source == airspeed_validated_s::SENSOR_3; + + const float airspeed_calibrated_from_sensor = airspeed_from_sensor ? airspeed_validated.calibrated_airspeed_m_s : NAN; + // Maximally allow the airspeed reading to be at FW_AIRSPD_MAX when arming. This is to catch very badly calibrated // airspeed sensors, but also high wind conditions that prevent a forward flight of the vehicle. float arming_max_airspeed_allowed = 20.f; @@ -62,7 +68,7 @@ void AirspeedChecks::checkAndReport(const Context &context, Report &reporter) /* * Check if airspeed is declared valid or not by airspeed selector. */ - if (!PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s)) { + if (!PX4_ISFINITE(airspeed_calibrated_from_sensor)) { /* EVENT */ @@ -75,7 +81,7 @@ void AirspeedChecks::checkAndReport(const Context &context, Report &reporter) } } - if (!context.isArmed() && fabsf(airspeed_validated.calibrated_airspeed_m_s) > arming_max_airspeed_allowed) { + if (!context.isArmed() && airspeed_calibrated_from_sensor > arming_max_airspeed_allowed) { /* EVENT * @description * Current airspeed reading too high. Check if wind is below maximum airspeed and redo airspeed diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index e939a56f54..de00c74b59 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -214,8 +214,10 @@ FixedwingPositionControl::airspeed_poll() _eas2tas = 1.0f; //this is the default value, taken in case of invalid airspeed + // do not use synthetic airspeed as this would create a thrust loop if (PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s) - && PX4_ISFINITE(airspeed_validated.true_airspeed_m_s)) { + && PX4_ISFINITE(airspeed_validated.true_airspeed_m_s) + && airspeed_validated.airspeed_source != airspeed_validated_s::SYNTHETIC) { airspeed_valid = true; diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index db077d6504..f54514ce50 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -93,10 +93,15 @@ bool FixedwingLandDetector::_get_landed_state() airspeed_validated_s airspeed_validated{}; _airspeed_validated_sub.copy(&airspeed_validated); + const bool airspeed_from_sensor = airspeed_validated.airspeed_source == airspeed_validated_s::SENSOR_1 + || airspeed_validated.airspeed_source == airspeed_validated_s::SENSOR_2 + || airspeed_validated.airspeed_source == airspeed_validated_s::SENSOR_3; + bool airspeed_invalid = false; - // 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) { + // set _airspeed_filtered to 0 if airspeed data is invalid or not from an actual airspeed sensor + if (!airspeed_from_sensor || !PX4_ISFINITE(airspeed_validated.true_airspeed_m_s) + || hrt_elapsed_time(&airspeed_validated.timestamp) > 1_s) { _airspeed_filtered = 0.0f; airspeed_invalid = true; diff --git a/src/modules/mavlink/streams/VFR_HUD.hpp b/src/modules/mavlink/streams/VFR_HUD.hpp index bbdfe0d3c8..76af7fc39e 100644 --- a/src/modules/mavlink/streams/VFR_HUD.hpp +++ b/src/modules/mavlink/streams/VFR_HUD.hpp @@ -82,9 +82,13 @@ private: airspeed_validated_s airspeed_validated{}; _airspeed_validated_sub.copy(&airspeed_validated); + const bool airspeed_from_sensor = airspeed_validated.airspeed_source == airspeed_validated_s::SENSOR_1 + || airspeed_validated.airspeed_source == airspeed_validated_s::SENSOR_2 + || airspeed_validated.airspeed_source == airspeed_validated_s::SENSOR_3; mavlink_vfr_hud_t msg{}; - msg.airspeed = airspeed_validated.calibrated_airspeed_m_s; + // display NAN in case of source not being one of the sensors + msg.airspeed = airspeed_from_sensor ? airspeed_validated.calibrated_airspeed_m_s : NAN; msg.groundspeed = sqrtf(lpos.vx * lpos.vx + lpos.vy * lpos.vy); msg.heading = math::degrees(matrix::wrap_2pi(lpos.heading));