diff --git a/libraries/AP_Notify/ToshibaLED_I2C.cpp b/libraries/AP_Notify/ToshibaLED_I2C.cpp index 4c0639cd94..4093653215 100644 --- a/libraries/AP_Notify/ToshibaLED_I2C.cpp +++ b/libraries/AP_Notify/ToshibaLED_I2C.cpp @@ -21,37 +21,35 @@ extern const AP_HAL::HAL& hal; -#define TOSHIBA_LED_ADDRESS 0x55 // default I2C bus address +#define TOSHIBA_LED_I2C_ADDR 0x55 // default I2C bus address +#define TOSHIBA_LED_I2C_BUS 1 + #define TOSHIBA_LED_PWM0 0x01 // pwm0 register #define TOSHIBA_LED_PWM1 0x02 // pwm1 register #define TOSHIBA_LED_PWM2 0x03 // pwm2 register #define TOSHIBA_LED_ENABLE 0x04 // enable register +ToshibaLED_I2C::ToshibaLED_I2C() + : _dev(hal.i2c_mgr->get_device(TOSHIBA_LED_I2C_BUS, TOSHIBA_LED_I2C_ADDR)) +{ +} + bool ToshibaLED_I2C::hw_init() { - // get pointer to i2c bus semaphore - AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore(); - // take i2c bus sempahore - if (!i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { return false; } - // disable recording of i2c lockup errors - hal.i2c->ignore_errors(true); - // enable the led - bool ret = (hal.i2c->writeRegister(TOSHIBA_LED_ADDRESS, TOSHIBA_LED_ENABLE, 0x03) == 0); + bool ret = _dev->write_register(TOSHIBA_LED_ENABLE, 0x03); // update the red, green and blue values to zero - uint8_t val[3] = { _led_off, _led_off, _led_off }; - ret &= (hal.i2c->writeRegisters(TOSHIBA_LED_ADDRESS, TOSHIBA_LED_PWM0, 3, val) == 0); - - // re-enable recording of i2c lockup errors - hal.i2c->ignore_errors(false); + uint8_t val[4] = { TOSHIBA_LED_PWM0, _led_off, _led_off, _led_off }; + ret &= _dev->transfer(val, sizeof(val), nullptr, 0); // give back i2c semaphore - i2c_sem->give(); + _dev->get_semaphore()->give(); return ret; } @@ -59,19 +57,11 @@ bool ToshibaLED_I2C::hw_init() // 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) { - // get pointer to i2c bus semaphore - AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore(); - - // exit immediately if we can't take the semaphore - if (i2c_sem == NULL || !i2c_sem->take(5)) { - return false; - } - // update the red value - uint8_t val[3] = { (uint8_t)(blue>>4), (uint8_t)(green>>4), (uint8_t)(red>>4) }; - bool success = (hal.i2c->writeRegisters(TOSHIBA_LED_ADDRESS, TOSHIBA_LED_PWM0, 3, val) == 0); + uint8_t val[4] = { TOSHIBA_LED_PWM0, (uint8_t)(blue>>4), (uint8_t)(green>>4), (uint8_t)(red>>4) }; + bool success = _dev->transfer(val, sizeof(val), nullptr, 0); // give back i2c semaphore - i2c_sem->give(); + _dev->get_semaphore()->give(); return success; } diff --git a/libraries/AP_Notify/ToshibaLED_I2C.h b/libraries/AP_Notify/ToshibaLED_I2C.h index 5f2d125073..3ddfc5e7b1 100644 --- a/libraries/AP_Notify/ToshibaLED_I2C.h +++ b/libraries/AP_Notify/ToshibaLED_I2C.h @@ -16,11 +16,16 @@ */ #pragma once +#include #include "ToshibaLED.h" class ToshibaLED_I2C : public ToshibaLED { public: + ToshibaLED_I2C(); bool hw_init(void); bool hw_set_rgb(uint8_t r, uint8_t g, uint8_t b); + +private: + AP_HAL::OwnPtr _dev; };