From 6da68e39f88358c48f97fc0d95a02003fd040d45 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 17 Jun 2021 16:03:35 +1000 Subject: [PATCH] AP_Airspeed: add rc channel option to disable all airspeed sensors --- libraries/AP_Airspeed/AP_Airspeed.cpp | 3 +++ libraries/AP_Airspeed/AP_Airspeed.h | 8 ++++++++ 2 files changed, 11 insertions(+) diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index 5aa3b5a1b9..e2c8f1dabc 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed.cpp @@ -623,6 +623,9 @@ void AP_Airspeed::Log_Airspeed() bool AP_Airspeed::use(uint8_t i) const { + if (_force_disable_use) { + return false; + } if (!enabled(i) || !param[i].use) { return false; } diff --git a/libraries/AP_Airspeed/AP_Airspeed.h b/libraries/AP_Airspeed/AP_Airspeed.h index 6234ffcf40..bf7c83c390 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.h +++ b/libraries/AP_Airspeed/AP_Airspeed.h @@ -96,6 +96,11 @@ public: bool use(uint8_t i) const; bool use(void) const { return use(primary); } + // force disabling of all airspeed sensors + void force_disable_use(bool value) { + _force_disable_use = value; + } + // return true if airspeed is enabled bool enabled(uint8_t i) const { if (i < AIRSPEED_MAX_SENSORS) { @@ -236,6 +241,9 @@ private: bool calibration_enabled; + // can be set to true to disable the use of the airspeed sensor + bool _force_disable_use; + // current primary sensor uint8_t primary; uint8_t num_sensors;