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

@@ -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();
+}

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());

libraries/AP_Airspeed/AP_Airspeed_MS4525.cpp

@@ -22,35 +22,27 @@
 #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
+// probe for a sensor
-bool AP_Airspeed_MS4525::init()
+bool AP_Airspeed_MS4525::probe(uint8_t bus, uint8_t address)
 {
-    const struct {
+    _dev = hal.i2c_mgr->get_device(bus, address);
-        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;
+        return false;
     }
     WITH_SEMAPHORE(_dev->get_semaphore());
 
@@ -61,16 +53,44 @@ bool AP_Airspeed_MS4525::init()
     hal.scheduler->delay(10);
     _collect();
 
-        found = (_last_sample_time_ms != 0);
+    return _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;
             }
         }
-    if (!found) {
+    } else {
-        printf("MS4525: no sensor found\n");
+        // 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;
+                }
+            }
+        }
+    }
+
+    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);

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);
 };