From bff985a9f4c3b564d87c67cd7e4726cbd089e9cd Mon Sep 17 00:00:00 2001
From: Michael du Breuil <wicked.shell.scripts@gmail.com>
Date: Wed, 18 Jul 2018 21:16:14 -0700
Subject: [PATCH] AP_Notify: Configure the PCA9685 every boot

---
 libraries/AP_Notify/PCA9685LED_I2C.cpp | 26 ++++++++++++++++++++++++++
 1 file changed, 26 insertions(+)

diff --git a/libraries/AP_Notify/PCA9685LED_I2C.cpp b/libraries/AP_Notify/PCA9685LED_I2C.cpp
index 30c077b758..731283f164 100644
--- a/libraries/AP_Notify/PCA9685LED_I2C.cpp
+++ b/libraries/AP_Notify/PCA9685LED_I2C.cpp
@@ -25,7 +25,10 @@
 #define NAVIO_LED_OFF    0xFF    // off
 
 #define PCA9685_ADDRESS 0x40
+#define PCA9685_MODE1 0x00
 #define PCA9685_PWM 0x6
+#define PCA9685_MODE_SLEEP          (1 << 4)
+#define PCA9685_MODE_AUTO_INCREMENT (1 << 5)
 
 extern const AP_HAL::HAL& hal;
 
@@ -42,6 +45,29 @@ bool PCA9685LED_I2C::hw_init()
         return false;
     }
 
+    _dev->get_semaphore()->take_blocking();
+
+    _dev->set_retries(5);
+
+    // read the current mode1 configuration
+    uint8_t mode1 = 0;
+    if (!_dev->read_registers(PCA9685_MODE1, &mode1, sizeof(mode1))) {
+        _dev->get_semaphore()->give();
+        return false;
+    }
+
+    // bring the device out of sleep, and enable auto register increment
+    uint8_t new_mode1 = (mode1 | PCA9685_MODE_AUTO_INCREMENT) & ~PCA9685_MODE_SLEEP;
+    const uint8_t config[2] = {PCA9685_MODE1, new_mode1};
+    if (!_dev->transfer(config, sizeof(config), nullptr, 0)) {
+        _dev->get_semaphore()->give();
+        return false;
+    }
+
+    _dev->set_retries(1);
+
+    _dev->get_semaphore()->give();
+
     _dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&PCA9685LED_I2C::_timer, void));
 
     return true;