aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Notify: Configure the PCA9685 every boot

Browse Source
This commit is contained in:
Michael du Breuil 2018-07-18 21:16:14 -07:00 committed by Andrew Tridgell
parent 961e16dca9
commit bff985a9f4
1 changed files with 26 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/AP_Notify/PCA9685LED_I2C.cpp
View File

@ -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;