From e6c7970a19e689f3d005ce31fcb3dc8c77cd4948 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell 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 #include #include +#include #include #include 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; iget_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 _dev; + + bool probe(uint8_t bus, uint8_t address); };