From 2a662793c2be2ffbc71c14459393273c49b88502 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 30 Jul 2020 15:29:55 +1000
Subject: [PATCH] AP_Compass: support probing RM3100 on all 4 addresses

---
 libraries/AP_Compass/AP_Compass.cpp      | 19 +++++++++++++++----
 libraries/AP_Compass/AP_Compass_RM3100.h |  5 ++++-
 2 files changed, 19 insertions(+), 5 deletions(-)

diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp
index b3b67a3200..e80c54cd07 100644
--- a/libraries/AP_Compass/AP_Compass.cpp
+++ b/libraries/AP_Compass/AP_Compass.cpp
@@ -1103,14 +1103,25 @@ void Compass::_probe_external_i2c_compasses(void)
                     true, ROTATION_NONE));
     }
 
+#ifdef HAL_COMPASS_RM3100_I2C_ADDR
+    const uint8_t rm3100_addresses[] = { HAL_COMPASS_RM3100_I2C_ADDR };
+#else
+    // RM3100 can be on 4 different addresses
+    const uint8_t rm3100_addresses[] = { HAL_COMPASS_RM3100_I2C_ADDR1,
+                                         HAL_COMPASS_RM3100_I2C_ADDR2,
+                                         HAL_COMPASS_RM3100_I2C_ADDR3,
+                                         HAL_COMPASS_RM3100_I2C_ADDR4 };
+#endif
     // external i2c bus
     FOREACH_I2C_EXTERNAL(i) {
-        ADD_BACKEND(DRIVER_RM3100, AP_Compass_RM3100::probe(GET_I2C_DEVICE(i, HAL_COMPASS_RM3100_I2C_ADDR),
-                    true, ROTATION_NONE));
+        for (uint8_t j=0; j<ARRAY_SIZE(rm3100_addresses); j++) {
+            ADD_BACKEND(DRIVER_RM3100, AP_Compass_RM3100::probe(GET_I2C_DEVICE(i, rm3100_addresses[j]), true, ROTATION_NONE));
+        }
     }
     FOREACH_I2C_INTERNAL(i) {
-        ADD_BACKEND(DRIVER_RM3100, AP_Compass_RM3100::probe(GET_I2C_DEVICE(i, HAL_COMPASS_RM3100_I2C_ADDR),
-                    all_external, ROTATION_NONE));
+        for (uint8_t j=0; j<ARRAY_SIZE(rm3100_addresses); j++) {
+            ADD_BACKEND(DRIVER_RM3100, AP_Compass_RM3100::probe(GET_I2C_DEVICE(i, rm3100_addresses[j]), all_external, ROTATION_NONE));
+        }
     }
 }
 
diff --git a/libraries/AP_Compass/AP_Compass_RM3100.h b/libraries/AP_Compass/AP_Compass_RM3100.h
index d97a6ddbf3..559792d3aa 100644
--- a/libraries/AP_Compass/AP_Compass_RM3100.h
+++ b/libraries/AP_Compass/AP_Compass_RM3100.h
@@ -23,7 +23,10 @@
 #include "AP_Compass_Backend.h"
 
 #ifndef HAL_COMPASS_RM3100_I2C_ADDR
-# define HAL_COMPASS_RM3100_I2C_ADDR 0x20
+# define HAL_COMPASS_RM3100_I2C_ADDR1 0x20
+# define HAL_COMPASS_RM3100_I2C_ADDR2 0x21
+# define HAL_COMPASS_RM3100_I2C_ADDR3 0x22
+# define HAL_COMPASS_RM3100_I2C_ADDR4 0x23
 #endif
 
 class AP_Compass_RM3100 : public AP_Compass_Backend