From e6c7970a19e689f3d005ce31fcb3dc8c77cd4948 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 30 Jun 2021 12:00:08 +1000
Subject: [PATCH] AP_Airspeed: support 3 I2C addresses for MS4525

and if bus number is configured then use only that bus number,
otherwise probe all buses as per existing behaviour
---
 libraries/AP_Airspeed/AP_Airspeed_Backend.cpp |  5 ++
 libraries/AP_Airspeed/AP_Airspeed_Backend.h   |  4 +
 libraries/AP_Airspeed/AP_Airspeed_MS4525.cpp  | 84 ++++++++++++-------
 libraries/AP_Airspeed/AP_Airspeed_MS4525.h    |  2 +
 4 files changed, 63 insertions(+), 32 deletions(-)

diff --git a/libraries/AP_Airspeed/AP_Airspeed_Backend.cpp b/libraries/AP_Airspeed/AP_Airspeed_Backend.cpp
index f6d1806039..441747bb4e 100644
--- a/libraries/AP_Airspeed/AP_Airspeed_Backend.cpp
+++ b/libraries/AP_Airspeed/AP_Airspeed_Backend.cpp
@@ -49,3 +49,8 @@ uint8_t AP_Airspeed_Backend::get_bus(void) const
 {
     return frontend.param[instance].bus;
 }
+
+bool AP_Airspeed_Backend::bus_is_confgured(void) const
+{
+    return frontend.param[instance].bus.configured();
+}
diff --git a/libraries/AP_Airspeed/AP_Airspeed_Backend.h b/libraries/AP_Airspeed/AP_Airspeed_Backend.h
index 59528d1586..a4470f1ee6 100644
--- a/libraries/AP_Airspeed/AP_Airspeed_Backend.h
+++ b/libraries/AP_Airspeed/AP_Airspeed_Backend.h
@@ -50,6 +50,10 @@ protected:
     int8_t get_pin(void) const;
     float get_psi_range(void) const;
     uint8_t get_bus(void) const;
+    bool bus_is_confgured(void) const;
+    uint8_t get_instance(void) const {
+        return instance;
+    }
 
     AP_Airspeed::pitot_tube_order get_tube_order(void) const {
         return AP_Airspeed::pitot_tube_order(frontend.param[instance].tube_order.get());
diff --git a/libraries/AP_Airspeed/AP_Airspeed_MS4525.cpp b/libraries/AP_Airspeed/AP_Airspeed_MS4525.cpp
index 0405b246f8..919559d360 100644
--- a/libraries/AP_Airspeed/AP_Airspeed_MS4525.cpp
+++ b/libraries/AP_Airspeed/AP_Airspeed_MS4525.cpp
@@ -22,55 +22,75 @@
 #include <AP_HAL/AP_HAL.h>
 #include <AP_HAL/I2CDevice.h>
 #include <AP_Math/AP_Math.h>
+#include <GCS_MAVLink/GCS.h>
 #include <stdio.h>
 #include <utility>
 
 extern const AP_HAL::HAL &hal;
 
-#define MS4525D0_I2C_ADDR 0x28
+#define MS4525D0_I2C_ADDR1 0x28
+#define MS4525D0_I2C_ADDR2 0x36
+#define MS4525D0_I2C_ADDR3 0x46
 
 AP_Airspeed_MS4525::AP_Airspeed_MS4525(AP_Airspeed &_frontend, uint8_t _instance) :
     AP_Airspeed_Backend(_frontend, _instance)
 {
 }
 
-// probe and initialise the sensor
-bool AP_Airspeed_MS4525::init()
+// probe for a sensor
+bool AP_Airspeed_MS4525::probe(uint8_t bus, uint8_t address)
 {
-    const struct {
-        uint8_t bus;
-        uint8_t addr;
-    } addresses[] = {
-        { 1, MS4525D0_I2C_ADDR },
-        { 0, MS4525D0_I2C_ADDR },
-        { 2, MS4525D0_I2C_ADDR },
-        { 3, MS4525D0_I2C_ADDR },
-    };
-    bool found = false;
-    for (uint8_t i=0; i<ARRAY_SIZE(addresses); i++) {
-        _dev = hal.i2c_mgr->get_device(addresses[i].bus, addresses[i].addr);
-        if (!_dev) {
-            continue;
-        }
-        WITH_SEMAPHORE(_dev->get_semaphore());
+    _dev = hal.i2c_mgr->get_device(bus, address);
+    if (!_dev) {
+        return false;
+    }
+    WITH_SEMAPHORE(_dev->get_semaphore());
 
-        // lots of retries during probe
-        _dev->set_retries(10);
+    // lots of retries during probe
+    _dev->set_retries(10);
     
-        _measure();
-        hal.scheduler->delay(10);
-        _collect();
+    _measure();
+    hal.scheduler->delay(10);
+    _collect();
+
+    return _last_sample_time_ms != 0;
+}
 
-        found = (_last_sample_time_ms != 0);
-        if (found) {
-            printf("MS4525: Found sensor on bus %u address 0x%02x\n", addresses[i].bus, addresses[i].addr);
-            break;
+// probe and initialise the sensor
+bool AP_Airspeed_MS4525::init()
+{
+    const uint8_t addresses[] = { MS4525D0_I2C_ADDR1, MS4525D0_I2C_ADDR2, MS4525D0_I2C_ADDR3 };
+    if (bus_is_confgured()) {
+        // the user has configured a specific bus
+        for (uint8_t addr : addresses) {
+            if (probe(get_bus(), addr)) {
+                goto found_sensor;
+            }
+        }
+    } else {
+        // if bus is not configured then fall back to the old
+        // behaviour of probing all buses, external first
+        FOREACH_I2C_EXTERNAL(bus) {
+            for (uint8_t addr : addresses) {
+                if (probe(bus, addr)) {
+                    goto found_sensor;
+                }
+            }
+        }
+        FOREACH_I2C_INTERNAL(bus) {
+            for (uint8_t addr : addresses) {
+                if (probe(bus, addr)) {
+                    goto found_sensor;
+                }
+            }
         }
     }
-    if (!found) {
-        printf("MS4525: no sensor found\n");
-        return false;
-    }
+
+    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MS4525[%u]: no sensor found", get_instance());
+    return false;
+
+found_sensor:
+    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MS4525[%u]: Found bus %u addr 0x%02x", get_instance(), _dev->bus_num(), _dev->get_bus_address());
 
     // drop to 2 retries for runtime
     _dev->set_retries(2);
diff --git a/libraries/AP_Airspeed/AP_Airspeed_MS4525.h b/libraries/AP_Airspeed/AP_Airspeed_MS4525.h
index 9a582e214d..be11ebf25d 100644
--- a/libraries/AP_Airspeed/AP_Airspeed_MS4525.h
+++ b/libraries/AP_Airspeed/AP_Airspeed_MS4525.h
@@ -58,4 +58,6 @@ private:
     uint32_t _last_sample_time_ms;
     uint32_t _measurement_started_ms;
     AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
+
+    bool probe(uint8_t bus, uint8_t address);
 };