From c5f488ed07332272393f8fca3d2ef401b2841f36 Mon Sep 17 00:00:00 2001
From: jflyper <shinoda@jaist.ac.jp>
Date: Mon, 14 Aug 2017 00:00:07 +0900
Subject: [PATCH] Restore zero i2c addr when detect failed

---
 src/main/drivers/barometer/barometer_bmp085.c |  7 +++++++
 src/main/drivers/barometer/barometer_bmp280.c |  5 +++++
 src/main/drivers/barometer/barometer_ms5611.c | 17 +++++++++++++----
 3 files changed, 25 insertions(+), 4 deletions(-)

diff --git a/src/main/drivers/barometer/barometer_bmp085.c b/src/main/drivers/barometer/barometer_bmp085.c
index 51082f9b8a..54259cda6d 100644
--- a/src/main/drivers/barometer/barometer_bmp085.c
+++ b/src/main/drivers/barometer/barometer_bmp085.c
@@ -169,6 +169,8 @@ bool bmp085Detect(const bmp085Config_t *config, baroDev_t *baro)
 {
     uint8_t data;
     bool ack;
+    bool defaultAddressApplied = false;
+
 #if defined(BARO_EOC_GPIO)
     IO_t eocIO = IO_NONE;
 #endif
@@ -200,6 +202,7 @@ bool bmp085Detect(const bmp085Config_t *config, baroDev_t *baro)
     if ((busdev->bustype == BUSTYPE_I2C) && (busdev->busdev_u.i2c.address == 0)) {
         // Default address for BMP085
         busdev->busdev_u.i2c.address = BMP085_I2C_ADDR;
+        defaultAddressApplied = true;
     }
 
     ack = bmp085ReadRegister(busdev, BMP085_CHIP_ID__REG, 1, &data); /* read Chip Id */
@@ -234,6 +237,10 @@ bool bmp085Detect(const bmp085Config_t *config, baroDev_t *baro)
 
     BMP085_OFF;
 
+    if (defaultAddressApplied) {
+        busdev->busdev_u.i2c.address = 0;
+    }
+
     return false;
 }
 
diff --git a/src/main/drivers/barometer/barometer_bmp280.c b/src/main/drivers/barometer/barometer_bmp280.c
index d914b37f9b..7388ab7a51 100644
--- a/src/main/drivers/barometer/barometer_bmp280.c
+++ b/src/main/drivers/barometer/barometer_bmp280.c
@@ -127,18 +127,23 @@ bool bmp280Detect(baroDev_t *baro)
     delay(20);
 
     busDevice_t *busdev = &baro->busdev;
+    bool defaultAddressApplied = false;
 
     bmp280BusInit(busdev);
 
     if ((busdev->bustype == BUSTYPE_I2C) && (busdev->busdev_u.i2c.address == 0)) {
         // Default address for BMP280
         busdev->busdev_u.i2c.address = BMP280_I2C_ADDR;
+        defaultAddressApplied = true;
     }
 
     bmp280ReadRegister(busdev, BMP280_CHIP_ID_REG, 1, &bmp280_chip_id);  /* read Chip Id */
 
     if (bmp280_chip_id != BMP280_DEFAULT_CHIP_ID) {
         bmp280BusDeinit(busdev);
+        if (defaultAddressApplied) {
+            busdev->busdev_u.i2c.address = 0;
+        }
         return false;
     }
 
diff --git a/src/main/drivers/barometer/barometer_ms5611.c b/src/main/drivers/barometer/barometer_ms5611.c
index d657cf9ea6..8cdec08ee5 100644
--- a/src/main/drivers/barometer/barometer_ms5611.c
+++ b/src/main/drivers/barometer/barometer_ms5611.c
@@ -122,6 +122,7 @@ bool ms5611Detect(baroDev_t *baro)
 {
     uint8_t sig;
     int i;
+    bool defaultAddressApplied = false;
 
     delay(10); // No idea how long the chip takes to power-up, but let's make it 10ms
 
@@ -132,11 +133,11 @@ bool ms5611Detect(baroDev_t *baro)
     if ((busdev->bustype == BUSTYPE_I2C) && (busdev->busdev_u.i2c.address == 0)) {
         // Default address for MS5611
         busdev->busdev_u.i2c.address = MS5611_I2C_ADDR;
+        defaultAddressApplied = true;
     }
 
     if (!ms5611ReadCommand(busdev, CMD_PROM_RD, 1, &sig) || sig == 0xFF) {
-        ms5611BusDeinit(busdev);
-        return false;
+        goto fail;
     }
 
     ms5611_reset(busdev);
@@ -147,8 +148,7 @@ bool ms5611Detect(baroDev_t *baro)
 
     // check crc, bail out if wrong - we are probably talking to BMP085 w/o XCLR line!
     if (ms5611_crc(ms5611_c) != 0) {
-        ms5611BusDeinit(busdev);
-        return false;
+        goto fail;
     }
 
     // TODO prom + CRC
@@ -161,6 +161,15 @@ bool ms5611Detect(baroDev_t *baro)
     baro->calculate = ms5611_calculate;
 
     return true;
+
+fail:;
+    ms5611BusDeinit(busdev);
+
+    if (defaultAddressApplied) {
+        busdev->busdev_u.i2c.address = 0;
+    }
+
+    return false;
 }
 
 static void ms5611_reset(busDevice_t *busdev)