From c9a830bf3f4f6fc3027c9e242e3766c38fcc3c01 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 7 Aug 2018 18:28:13 +1000
Subject: [PATCH] AP_Notify: support both NCP5623 addresses

---
 libraries/AP_Notify/AP_Notify.cpp |  4 ++-
 libraries/AP_Notify/NCP5623.cpp   | 55 +++++++++++++++++--------------
 2 files changed, 33 insertions(+), 26 deletions(-)

diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp
index 2f494f6937..68243a9303 100644
--- a/libraries/AP_Notify/AP_Notify.cpp
+++ b/libraries/AP_Notify/AP_Notify.cpp
@@ -241,7 +241,9 @@ void AP_Notify::add_backends(void)
                 break;
 #if !HAL_MINIMIZE_FEATURES
             case Notify_LED_NCP5623_I2C_External:
-                ADD_BACKEND(new NCP5623(TOSHIBA_LED_I2C_BUS_EXTERNAL));
+                FOREACH_I2C_EXTERNAL(b) {
+                    ADD_BACKEND(new NCP5623(b));
+                }
                 break;
             case Notify_LED_NCP5623_I2C_Internal:
                 ADD_BACKEND(new NCP5623(TOSHIBA_LED_I2C_BUS_INTERNAL));
diff --git a/libraries/AP_Notify/NCP5623.cpp b/libraries/AP_Notify/NCP5623.cpp
index b2feba9218..986ff88f93 100644
--- a/libraries/AP_Notify/NCP5623.cpp
+++ b/libraries/AP_Notify/NCP5623.cpp
@@ -29,6 +29,7 @@ extern const AP_HAL::HAL& hal;
 #define NCP5623_LED_OFF     0x00    // off
 
 #define NCP5623_LED_I2C_ADDR 0x38    // default I2C bus address
+#define NCP5623_C_LED_I2C_ADDR 0x39    // default I2C bus address for the NCP5623C
 
 #define NCP5623_LED_PWM0    0x40    // pwm0 register
 #define NCP5623_LED_PWM1    0x60    // pwm1 register
@@ -62,36 +63,40 @@ bool NCP5623::write_pwm(uint8_t _rgb[3])
 
 bool NCP5623::hw_init(void)
 {
-    // first look for led on external bus
-    _dev = std::move(hal.i2c_mgr->get_device(_bus, NCP5623_LED_I2C_ADDR));
-    if (!_dev) {
-        return false;
-    }
+    uint8_t addrs[] = { NCP5623_LED_I2C_ADDR, NCP5623_C_LED_I2C_ADDR };
+    for (uint8_t i=0; i<ARRAY_SIZE(addrs); i++) {
+        // first look for led on external bus
+        _dev = std::move(hal.i2c_mgr->get_device(_bus, addrs[i]));
+        if (!_dev) {
+            continue;
+        }
 
-    _dev->get_semaphore()->take_blocking();
-    _dev->set_retries(10);
+        _dev->get_semaphore()->take_blocking();
+        _dev->set_retries(10);
 
-    // enable the led
-    bool ret = write(NCP5623_LED_ENABLE, 0x1f);
-    if (!ret) {
+        // enable the led
+        bool ret = write(NCP5623_LED_ENABLE, 0x1f);
+        if (!ret) {
+            _dev->get_semaphore()->give();
+            continue;
+        }
+
+        // update the red, green and blue values to zero
+        uint8_t off[3] = { _led_off, _led_off, _led_off };
+        ret = write_pwm(off);
+
+        _dev->set_retries(1);
+
+        // give back i2c semaphore
         _dev->get_semaphore()->give();
-        return false;
+
+        if (ret) {
+            _dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&NCP5623::_timer, void));
+        }
+        return true;
     }
 
-    // update the red, green and blue values to zero
-    uint8_t off[3] = { _led_off, _led_off, _led_off };
-    ret = write_pwm(off);
-
-    _dev->set_retries(1);
-
-    // give back i2c semaphore
-    _dev->get_semaphore()->give();
-
-    if (ret) {
-        _dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&NCP5623::_timer, void));
-    }
-
-    return ret;
+    return false;
 }
 
 // set_rgb - set color as a combination of red, green and blue values