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

AP_Notify: fix ToshibaLED_I2C and check both I2C buses

Browse Source 
the rgb value was never being set meaning the led was always off
external bus is checked first
This commit is contained in:
Randy Mackay 2017-01-26 18:11:37 +09:00
parent 0e20c8040c
commit eedfe13f9c
1 changed files with 17 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/AP_Notify/ToshibaLED_I2C.cpp
View File

@ -27,7 +27,8 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
#define TOSHIBA_LED_I2C_ADDR 0x55 // default I2C bus address #define TOSHIBA_LED_I2C_ADDR 0x55 // default I2C bus address
#define TOSHIBA_LED_I2C_BUS 1 #define TOSHIBA_LED_I2C_BUS_INTERNAL 0
#define TOSHIBA_LED_I2C_BUS_EXTERNAL 1
#define TOSHIBA_LED_PWM0 0x01 // pwm0 register #define TOSHIBA_LED_PWM0 0x01 // pwm0 register
#define TOSHIBA_LED_PWM1 0x02 // pwm1 register #define TOSHIBA_LED_PWM1 0x02 // pwm1 register
@ -36,9 +37,8 @@ extern const AP_HAL::HAL& hal;
bool ToshibaLED_I2C::hw_init() bool ToshibaLED_I2C::hw_init()
{ {
_dev = std::move(hal.i2c_mgr->get_device(TOSHIBA_LED_I2C_BUS, TOSHIBA_LED_I2C_ADDR)); // first look for led on external bus
_dev = std::move(hal.i2c_mgr->get_device(TOSHIBA_LED_I2C_BUS_EXTERNAL, TOSHIBA_LED_I2C_ADDR));
// take i2c bus sempahore
if (!_dev || !_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { if (!_dev || !_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return false; return false;
} }
@ -46,6 +46,18 @@ bool ToshibaLED_I2C::hw_init()
// enable the led // enable the led
bool ret = _dev->write_register(TOSHIBA_LED_ENABLE, 0x03); bool ret = _dev->write_register(TOSHIBA_LED_ENABLE, 0x03);
// on failure try the internal bus
if (!ret) {
// give back external bus semaphore
_dev->get_semaphore()->give();
// get internal I2C bus driver
_dev = std::move(hal.i2c_mgr->get_device(TOSHIBA_LED_I2C_BUS_INTERNAL, TOSHIBA_LED_I2C_ADDR));
if (!_dev || !_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return false;
}
ret = _dev->write_register(TOSHIBA_LED_ENABLE, 0x03);
}
// update the red, green and blue values to zero // update the red, green and blue values to zero
uint8_t val[4] = { TOSHIBA_LED_PWM0, _led_off, _led_off, _led_off }; uint8_t val[4] = { TOSHIBA_LED_PWM0, _led_off, _led_off, _led_off };
ret &= _dev->transfer(val, sizeof(val), nullptr, 0); ret &= _dev->transfer(val, sizeof(val), nullptr, 0);
@ -61,6 +73,7 @@ bool ToshibaLED_I2C::hw_init()
// set_rgb - set color as a combination of red, green and blue values // set_rgb - set color as a combination of red, green and blue values
bool ToshibaLED_I2C::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) bool ToshibaLED_I2C::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
{ {
rgb = {red, green, blue};
_need_update = true; _need_update = true;
return true; return true;
} }