rgbled: fixed detection of device on PX4v1

There is a serial EEPROM on the PX4IOv1 board that answers on I2C
address 0x55. We need some extra I2C transfers to ensure we are
talking to a real RGBLED device.

1 files changed, 14 insertions, 1 deletions

diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp
index ea87b37d9..34a45c739 100644
--- a/src/drivers/rgbled/rgbled.cpp
+++ b/src/drivers/rgbled/rgbled.cpp
@@ -172,7 +172,20 @@ RGBLED::probe()
 	bool on, powersave;
 	uint8_t r, g, b;
 
-	ret = get(on, powersave, r, g, b);
+	/**
+	   this may look strange, but is needed. There is a serial
+	   EEPROM (Microchip-24aa01) on the PX4FMU-v1 that responds to
+	   a bunch of I2C addresses, including the 0x55 used by this
+	   LED device. So we need to do enough operations to be sure
+	   we are talking to the right device. These 3 operations seem
+	   to be enough, as the 3rd one consistently fails if no
+	   RGBLED is on the bus.
+	 */
+	if ((ret=get(on, powersave, r, g, b)) != OK ||
+	    (ret=send_led_enable(false) != OK) ||
+	    (ret=send_led_enable(false) != OK)) {
+		return ret;
+	}
 
 	return ret;
 }