From b2def13d6f6cc744f565657d87e554b9b5223959 Mon Sep 17 00:00:00 2001
From: InspiredBrandon <65676272+InspiredBrandon@users.noreply.github.com>
Date: Mon, 2 Aug 2021 17:36:09 -0700
Subject: [PATCH] NCP5623c RGB LED driver I2C address auto detect

 * Enables the LED on mRobotics GPS receivers
 * Probes I2C addresses and sets colors accordingly
---
 .../rgbled_ncp5623c/rgbled_ncp5623c.cpp       | 32 +++++++++++++++----
 .../rgbled_ncp5623c/rgbled_ncp5623c_params.c  |  5 ++-
 2 files changed, 28 insertions(+), 9 deletions(-)

diff --git a/src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c.cpp b/src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c.cpp
index 76aff089a9..8a670aaebf 100755
--- a/src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c.cpp
+++ b/src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c.cpp
@@ -52,7 +52,8 @@
 
 using namespace time_literals;
 
-#define ADDR			0x39	/**< I2C adress of NCP5623C */
+#define ADDR			0x39	/**< I2C address of NCP5623C */
+#define ALT_ADDR		0x38	/**< Alternative I2C address of NCP5623C */
 
 #define NCP5623_LED_CURRENT	0x20	/**< Current register */
 #define NCP5623_LED_PWM0	0x40	/**< pwm0 register */
@@ -96,11 +97,18 @@ private:
 	void			update_params();
 
 	int			write(uint8_t reg, uint8_t data);
+
+	uint8_t			red;
+	uint8_t			green;
+	uint8_t			blue;
 };
 
 RGBLED_NCP5623C::RGBLED_NCP5623C(const I2CSPIDriverConfig &config) :
 	I2C(config),
-	I2CSPIDriver(config)
+	I2CSPIDriver(config),
+	red(NCP5623_LED_PWM0),
+	green(NCP5623_LED_PWM1),
+	blue(NCP5623_LED_PWM2)
 {
 }
 
@@ -136,9 +144,21 @@ RGBLED_NCP5623C::init()
 int
 RGBLED_NCP5623C::probe()
 {
+	int status = PX4_ERROR;
 	_retries = 4;
+	status = write(NCP5623_LED_CURRENT, NCP5623_LED_OFF);
 
-	return write(NCP5623_LED_CURRENT, 0x00);
+	if (status == PX4_ERROR) {
+		set_device_address(ALT_ADDR);
+		status = write(NCP5623_LED_CURRENT, NCP5623_LED_OFF);
+
+		if (status == PX4_OK) {
+			red = NCP5623_LED_PWM2;
+			blue = NCP5623_LED_PWM0;
+		}
+	}
+
+	return status;
 }
 
 void
@@ -215,9 +235,9 @@ RGBLED_NCP5623C::send_led_rgb()
 	uint8_t brightness = 0x1f * _max_brightness;
 
 	msg[0] = NCP5623_LED_CURRENT | (brightness & 0x1f);
-	msg[2] = NCP5623_LED_PWM0 | (uint8_t(_r * _brightness) & 0x1f);
-	msg[4] = NCP5623_LED_PWM1 | (uint8_t(_g * _brightness) & 0x1f);
-	msg[6] = NCP5623_LED_PWM2 | (uint8_t(_b * _brightness) & 0x1f);
+	msg[2] = red | (uint8_t(_r * _brightness) & 0x1f);
+	msg[4] = green | (uint8_t(_g * _brightness) & 0x1f);
+	msg[6] = blue | (uint8_t(_b * _brightness) & 0x1f);
 
 	return transfer(&msg[0], 7, nullptr, 0);
 }
diff --git a/src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c_params.c b/src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c_params.c
index a834cae4cb..6a677bf95d 100755
--- a/src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c_params.c
+++ b/src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c_params.c
@@ -6,9 +6,9 @@
  * modification, are permitted provided that the following conditions
  * are met:
  *
- * 1. Redistributions of source code must retain the above copyright
+ * 1. Redistribution of source code must retain the above copyright
  *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
+ * 2. Redistribution in binary form must reproduce the above copyright
  *    notice, this list of conditions and the following disclaimer in
  *    the documentation and/or other materials provided with the
  *    distribution.
@@ -53,4 +53,3 @@
  * @group System
  */
 PARAM_DEFINE_INT32(LED_RGB1_MAXBRT, 31);
-