From a867e041168f4882ae85494dc594600383496f43 Mon Sep 17 00:00:00 2001
From: Nathan Costa <nathan.costa@hiteccs.com>
Date: Tue, 1 Feb 2022 15:39:43 -0500
Subject: [PATCH] AP_Periph: airspeed add support for CAN/I2C switch and LED

---
 Tools/AP_Periph/AP_Periph.cpp | 19 +++++++++++++++++--
 1 file changed, 17 insertions(+), 2 deletions(-)

diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp
index 1f525e3cfa..20656b13d3 100644
--- a/Tools/AP_Periph/AP_Periph.cpp
+++ b/Tools/AP_Periph/AP_Periph.cpp
@@ -27,6 +27,7 @@
 #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
 #include <AP_HAL_ChibiOS/hwdef/common/stm32_util.h>
 #include <AP_HAL_ChibiOS/hwdef/common/watchdog.h>
+#include <AP_HAL_ChibiOS/I2CDevice.h>
 #endif
 
 extern const AP_HAL::HAL &hal;
@@ -180,10 +181,24 @@ void AP_Periph_FW::init()
 #endif
 
 #ifdef HAL_PERIPH_ENABLE_AIRSPEED
-    if (airspeed.enabled()) {
-        // Note: logging of ARSPD is not enabled currently. To enable, call airspeed.set_log_bit();
+    if (airspeed.enabled()){
+#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
+        const bool pins_enabled = ChibiOS::I2CBus::check_select_pins(0x01);
+        if (pins_enabled) {
+            ChibiOS::I2CBus::set_bus_to_floating(0);
+#ifdef HAL_GPIO_PIN_LED_CAN_I2C
+            palWriteLine(HAL_GPIO_PIN_LED_CAN_I2C, 1);
+#endif
+        } else {
+            // Note: logging of ARSPD is not enabled currently. To enable, call airspeed.set_log_bit(); here
+            airspeed.init();
+        }
+#else
+        // Note: logging of ARSPD is not enabled currently. To enable, call airspeed.set_log_bit(); here
         airspeed.init();
+#endif
     }
+
 #endif
 
 #ifdef HAL_PERIPH_ENABLE_RANGEFINDER