From 697131e94adb588dee895cb74b9e7ca07e5d3698 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 6 Sep 2017 20:12:36 +1000 Subject: [PATCH] AP_Notify: support dual I2c toshiba LEDs This restructures AP_Notify to allow for multiple backends of the same type. --- libraries/AP_Notify/AP_Notify.cpp | 207 ++++++++++-------- libraries/AP_Notify/AP_Notify.h | 4 + libraries/AP_Notify/ToshibaLED_I2C.cpp | 34 +-- libraries/AP_Notify/ToshibaLED_I2C.h | 3 + .../ToshibaLED_test/ToshibaLED_test.cpp | 2 +- 5 files changed, 138 insertions(+), 112 deletions(-) diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp index fc7ea1c7fe..662b0f5c6f 100644 --- a/libraries/AP_Notify/AP_Notify.cpp +++ b/libraries/AP_Notify/AP_Notify.cpp @@ -34,7 +34,10 @@ AP_Notify *AP_Notify::_instance; -#define CONFIG_NOTIFY_DEVICES_COUNT 5 +#define CONFIG_NOTIFY_DEVICES_MAX 6 + +#define TOSHIBA_LED_I2C_BUS_INTERNAL 0 +#define TOSHIBA_LED_I2C_BUS_EXTERNAL 1 // table of user settable parameters const AP_Param::GroupInfo AP_Notify::var_info[] = { @@ -91,100 +94,114 @@ AP_Notify::AP_Notify() struct AP_Notify::notify_flags_and_values_type AP_Notify::flags; struct AP_Notify::notify_events_type AP_Notify::events; -NotifyDevice *AP_Notify::_devices[] = {nullptr, nullptr, nullptr, nullptr, nullptr}; +NotifyDevice *AP_Notify::_devices[CONFIG_NOTIFY_DEVICES_MAX]; +uint8_t AP_Notify::_num_devices; + +#define ADD_BACKEND(backend) do { _devices[_num_devices++] = backend; if (_num_devices >= CONFIG_NOTIFY_DEVICES_MAX) return;} while(0) + +// add notify backends to _devices array +void AP_Notify::add_backends(void) +{ + if (_num_devices != 0) { + return; + } + +// Notify devices for PX4 boards +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 + #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_PX4_V3 // Has enough memory for Oreo LEDs + ADD_BACKEND(new AP_BoardLED()); + ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL)); + ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_INTERNAL)); + ADD_BACKEND(new ToneAlarm_PX4()); + ADD_BACKEND(new Display()); + + // Oreo LED enable/disable by NTF_OREO_THEME parameter + if (_oreo_theme) { + ADD_BACKEND(new OreoLED_PX4(_oreo_theme)); + } + + #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_PX4_V4 // Has its own LED board + ADD_BACKEND(new PixRacerLED()); + ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL)); + ADD_BACKEND(new ToneAlarm_PX4()); + ADD_BACKEND(new Display()); + + #else // All other px4 boards use standard devices. + ADD_BACKEND(new AP_BoardLED()); + ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL)); + ADD_BACKEND(new ToneAlarm_PX4()); + ADD_BACKEND(new Display()); + #endif + +// Notify devices for VRBRAIN boards +#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN + #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_VRBRAIN_V45 // Uses px4 LED board + ADD_BACKEND(new AP_BoardLED()); + ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL)); + ADD_BACKEND(new ToneAlarm_PX4()); + ADD_BACKEND(new ExternalLED()); + #else + ADD_BACKEND(new VRBoard_LED()); + ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL)); + ADD_BACKEND(new ToneAlarm_PX4()); + ADD_BACKEND(new ExternalLED()); + #endif + +// Notify devices for linux boards +#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX + #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO + ADD_BACKEND(new NavioLED_I2C()); + ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL)); + + #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO2 + ADD_BACKEND(new DiscreteRGBLed(4, 27, 6, false)); + ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL)); + + #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI + ADD_BACKEND(new AP_BoardLED()); + ADD_BACKEND(new Buzzer()); + ADD_BACKEND(new Display()); + + #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE + ADD_BACKEND(new AP_BoardLED()); + ADD_BACKEND(new Display()); + + #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT + ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL)); + ADD_BACKEND(new ToneAlarm_Linux()); + + #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE + ADD_BACKEND(new RCOutputRGBLedOff(15, 13, 14, 255)); + + #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 || \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI + ADD_BACKEND(new AP_BoardLED()); + + #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH + ADD_BACKEND(new AP_BoardLED()); + ADD_BACKEND(new RCOutputRGBLed(HAL_RCOUT_RGBLED_RED, HAL_RCOUT_RGBLED_GREEN, HAL_RCOUT_RGBLED_BLUE)); + + #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO + ADD_BACKEND(new DiscoLED()); + ADD_BACKEND(new ToneAlarm_Linux()); + + #else // other linux + ADD_BACKEND(new AP_BoardLED()); + ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL)); + ADD_BACKEND(new ToneAlarm_Linux()); + #endif + +#else + ADD_BACKEND(new AP_BoardLED()); + ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL)); +#endif +} // initialisation void AP_Notify::init(bool enable_external_leds) { - -// Notify devices for PX4 boards -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 - #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_PX4_V3 // Has enough memory for Oreo LEDs - _devices[0] = new AP_BoardLED(); - _devices[1] = new ToshibaLED_I2C(); - _devices[2] = new ToneAlarm_PX4(); - _devices[3] = new Display(); - - // Oreo LED enable/disable by NTF_OREO_THEME parameter - if (_oreo_theme) { - _devices[4] = new OreoLED_PX4(_oreo_theme); - } - - #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_PX4_V4 // Has its own LED board - _devices[0] = new PixRacerLED(); - _devices[1] = new ToshibaLED_I2C(); - _devices[2] = new ToneAlarm_PX4(); - _devices[3] = new Display(); - - #else // All other px4 boards use standard devices. - _devices[0] = new AP_BoardLED(); - _devices[1] = new ToshibaLED_I2C(); - _devices[2] = new ToneAlarm_PX4(); - _devices[3] = new Display(); - #endif - -// Notify devices for VRBRAIN boards -#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_VRBRAIN_V45 // Uses px4 LED board - _devices[0] = new AP_BoardLED(); - _devices[1] = new ToshibaLED_I2C(); - _devices[2] = new ToneAlarm_PX4(); - _devices[3] = new ExternalLED(); - #else - _devices[0] = new VRBoard_LED(); - _devices[1] = new ToshibaLED_I2C(); - _devices[2] = new ToneAlarm_PX4(); - _devices[3] = new ExternalLED(); - #endif - -// Notify devices for linux boards -#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX - #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO - _devices[0] = new NavioLED_I2C(); - _devices[1] = new ToshibaLED_I2C(); - - #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO2 - _devices[0] = new DiscreteRGBLed(4, 27, 6, false); - _devices[1] = new ToshibaLED_I2C(); - - #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI - _devices[0] = new AP_BoardLED(); - _devices[1] = new Buzzer(); - _devices[2] = new Display(); - - #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE - _devices[0] = new AP_BoardLED(); - _devices[1] = new Display(); - - #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT - _devices[0] = new ToshibaLED_I2C(); - _devices[1] = new ToneAlarm_Linux(); - - #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE - _devices[0] = new RCOutputRGBLedOff(15, 13, 14, 255); - - #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 || \ - CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI - _devices[0] = new AP_BoardLED(); - - #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH - _devices[0] = new AP_BoardLED(); - _devices[1] = new RCOutputRGBLed(HAL_RCOUT_RGBLED_RED, HAL_RCOUT_RGBLED_GREEN, HAL_RCOUT_RGBLED_BLUE); - - #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO - _devices[0] = new DiscoLED(); - _devices[1] = new ToneAlarm_Linux(); - - #else - _devices[0] = new AP_BoardLED(); - _devices[1] = new ToshibaLED_I2C(); - _devices[2] = new ToneAlarm_Linux(); - #endif - -#else - _devices[0] = new AP_BoardLED(); - _devices[1] = new ToshibaLED_I2C(); -#endif + // add all the backends + add_backends(); // clear all flags and events memset(&AP_Notify::flags, 0, sizeof(AP_Notify::flags)); @@ -197,7 +214,7 @@ void AP_Notify::init(bool enable_external_leds) AP_Notify::flags.external_leds = enable_external_leds; - for (uint8_t i = 0; i < CONFIG_NOTIFY_DEVICES_COUNT; i++) { + for (uint8_t i = 0; i < _num_devices; i++) { if (_devices[i] != nullptr) { _devices[i]->pNotify = this; _devices[i]->init(); @@ -208,7 +225,7 @@ void AP_Notify::init(bool enable_external_leds) // main update function, called at 50Hz void AP_Notify::update(void) { - for (uint8_t i = 0; i < CONFIG_NOTIFY_DEVICES_COUNT; i++) { + for (uint8_t i = 0; i < _num_devices; i++) { if (_devices[i] != nullptr) { _devices[i]->update(); } @@ -221,7 +238,7 @@ void AP_Notify::update(void) // handle a LED_CONTROL message void AP_Notify::handle_led_control(mavlink_message_t *msg) { - for (uint8_t i = 0; i < CONFIG_NOTIFY_DEVICES_COUNT; i++) { + for (uint8_t i = 0; i < _num_devices; i++) { if (_devices[i] != nullptr) { _devices[i]->handle_led_control(msg); } @@ -231,7 +248,7 @@ void AP_Notify::handle_led_control(mavlink_message_t *msg) // handle a PLAY_TUNE message void AP_Notify::handle_play_tune(mavlink_message_t *msg) { - for (uint8_t i = 0; i < CONFIG_NOTIFY_DEVICES_COUNT; i++) { + for (uint8_t i = 0; i < _num_devices; i++) { if (_devices[i] != nullptr) { _devices[i]->handle_play_tune(msg); } diff --git a/libraries/AP_Notify/AP_Notify.h b/libraries/AP_Notify/AP_Notify.h index 7d223adf93..1c187e5705 100644 --- a/libraries/AP_Notify/AP_Notify.h +++ b/libraries/AP_Notify/AP_Notify.h @@ -115,6 +115,9 @@ public: // initialisation void init(bool enable_external_leds); + // add all backends + void add_backends(void); + /// update - allow updates of leds that cannot be updated during a timed interrupt void update(void); @@ -152,4 +155,5 @@ private: char _flight_mode_str[5]; static NotifyDevice* _devices[]; + static uint8_t _num_devices; }; diff --git a/libraries/AP_Notify/ToshibaLED_I2C.cpp b/libraries/AP_Notify/ToshibaLED_I2C.cpp index 5cc1b1df76..2cd8b6d25c 100644 --- a/libraries/AP_Notify/ToshibaLED_I2C.cpp +++ b/libraries/AP_Notify/ToshibaLED_I2C.cpp @@ -27,46 +27,48 @@ extern const AP_HAL::HAL& hal; #define TOSHIBA_LED_I2C_ADDR 0x55 // default I2C bus address -#define TOSHIBA_LED_I2C_BUS_INTERNAL 0 -#define TOSHIBA_LED_I2C_BUS_EXTERNAL 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 -bool ToshibaLED_I2C::hw_init() +ToshibaLED_I2C::ToshibaLED_I2C(uint8_t bus) + : ToshibaLED() + , _bus(bus) +{ +} + +bool ToshibaLED_I2C::hw_init(void) { // first look for led on external bus - _dev = std::move(hal.i2c_mgr->get_device(TOSHIBA_LED_I2C_BUS_EXTERNAL, TOSHIBA_LED_I2C_ADDR)); + _dev = std::move(hal.i2c_mgr->get_device(_bus, TOSHIBA_LED_I2C_ADDR)); if (!_dev || !_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { return false; } + _dev->set_retries(10); + // enable the led 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); + return false; } // update the red, green and blue values to zero 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); + + _dev->set_retries(1); // give back i2c semaphore _dev->get_semaphore()->give(); - _dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&ToshibaLED_I2C::_timer, void)); - + if (ret) { + _dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&ToshibaLED_I2C::_timer, void)); + } + return ret; } diff --git a/libraries/AP_Notify/ToshibaLED_I2C.h b/libraries/AP_Notify/ToshibaLED_I2C.h index cd33723e6f..adddaa1f79 100644 --- a/libraries/AP_Notify/ToshibaLED_I2C.h +++ b/libraries/AP_Notify/ToshibaLED_I2C.h @@ -21,6 +21,8 @@ class ToshibaLED_I2C : public ToshibaLED { +public: + ToshibaLED_I2C(uint8_t bus); protected: bool hw_init(void) override; bool hw_set_rgb(uint8_t r, uint8_t g, uint8_t b) override; @@ -32,4 +34,5 @@ private: struct { uint8_t r, g, b; } rgb; + uint8_t _bus; }; diff --git a/libraries/AP_Notify/examples/ToshibaLED_test/ToshibaLED_test.cpp b/libraries/AP_Notify/examples/ToshibaLED_test/ToshibaLED_test.cpp index 2de522d7e2..56371d37df 100644 --- a/libraries/AP_Notify/examples/ToshibaLED_test/ToshibaLED_test.cpp +++ b/libraries/AP_Notify/examples/ToshibaLED_test/ToshibaLED_test.cpp @@ -9,7 +9,7 @@ void blink(); const AP_HAL::HAL& hal = AP_HAL::get_HAL(); -static ToshibaLED_I2C toshiba_led; +static ToshibaLED_I2C toshiba_led(1); void setup(void) {