AP_Notify: Configure the PCA9685 every boot

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;