From a8bf2c014161349b5e987a3356d1e2df164f17e6 Mon Sep 17 00:00:00 2001
From: Mohammad Hefny <mohammad.hefny@gmail.com>
Date: Sun, 12 Jun 2022 03:59:54 +0200
Subject: [PATCH] fix: baro same driver multi-definition

---
 libraries/AP_Baro/AP_Baro.cpp        | 17 ++++++++++++++++-
 libraries/AP_Baro/AP_Baro.h          |  2 ++
 libraries/AP_Baro/AP_Baro_BMP085.cpp |  3 +++
 3 files changed, 21 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp
index c0120c8a8e..5ca798c386 100644
--- a/libraries/AP_Baro/AP_Baro.cpp
+++ b/libraries/AP_Baro/AP_Baro.cpp
@@ -532,6 +532,21 @@ bool AP_Baro::_add_backend(AP_Baro_Backend *backend)
     return true;
 }
 
+/*
+  wrapper around hal.i2c_mgr->get_device() that prevents duplicate devices being opened
+ */
+bool AP_Baro::_have_i2c_driver(uint8_t bus, uint8_t address) const
+{
+    for (int i=0; i<_num_drivers; ++i) {
+        if (AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_I2C, bus, address, 0) ==
+            AP_HAL::Device::change_bus_id(uint32_t(sensors[i].bus_id.get()), 0)) {
+            // device already has been defined.
+            return true;
+        }
+    }
+    return false;
+}
+
 /*
   macro to add a backend with check for too many sensors
  We don't try to start more than the maximum allowed
@@ -589,7 +604,7 @@ void AP_Baro::init(void)
 #endif
 
 // macro for use by HAL_INS_PROBE_LIST
-#define GET_I2C_DEVICE(bus, address) hal.i2c_mgr->get_device(bus, address)
+#define GET_I2C_DEVICE(bus, address) _have_i2c_driver(bus, address)?nullptr:hal.i2c_mgr->get_device(bus, address)
 
 #if AP_SIM_BARO_ENABLED
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL && AP_BARO_MS56XX_ENABLED
diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h
index e94fa07d92..3d77f8b741 100644
--- a/libraries/AP_Baro/AP_Baro.h
+++ b/libraries/AP_Baro/AP_Baro.h
@@ -292,6 +292,8 @@ private:
     // when did we last notify the GCS of new pressure reference?
     uint32_t                            _last_notify_ms;
 
+    // see if we already have probed a i2c driver by bus number and address
+    bool _have_i2c_driver(uint8_t bus_num, uint8_t address) const;
     bool _add_backend(AP_Baro_Backend *backend);
     void _probe_i2c_barometers(void);
     AP_Int8                            _filter_range;  // valid value range from mean value
diff --git a/libraries/AP_Baro/AP_Baro_BMP085.cpp b/libraries/AP_Baro/AP_Baro_BMP085.cpp
index 0f8ba2d7a9..9601db80d2 100644
--- a/libraries/AP_Baro/AP_Baro_BMP085.cpp
+++ b/libraries/AP_Baro/AP_Baro_BMP085.cpp
@@ -59,6 +59,9 @@ AP_Baro_Backend * AP_Baro_BMP085::probe(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::De
 
 bool AP_Baro_BMP085::_init()
 {
+    if (!_dev) {
+        return false;
+    }
     union {
         uint8_t buff[22];
         uint16_t wb[11];