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

@@ -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;
 
-// initialisation
-void AP_Notify::init(bool enable_external_leds)
+#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
-        _devices[0] = new AP_BoardLED();
-        _devices[1] = new ToshibaLED_I2C();
-        _devices[2] = new ToneAlarm_PX4();
-        _devices[3] = new Display();
+    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) {
-            _devices[4] = new OreoLED_PX4(_oreo_theme);
+        ADD_BACKEND(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();
+    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.
-        _devices[0] = new AP_BoardLED();
-        _devices[1] = new ToshibaLED_I2C();
-        _devices[2] = new ToneAlarm_PX4();
-        _devices[3] = new Display();
+    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
-        _devices[0] = new AP_BoardLED();
-        _devices[1] = new ToshibaLED_I2C();
-        _devices[2] = new ToneAlarm_PX4();
-        _devices[3] = new ExternalLED();
+    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
-        _devices[0] = new VRBoard_LED();
-        _devices[1] = new ToshibaLED_I2C();
-        _devices[2] = new ToneAlarm_PX4();
-        _devices[3] = new ExternalLED();
+    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
-        _devices[0] = new NavioLED_I2C();
-        _devices[1] = new ToshibaLED_I2C();
+    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
-        _devices[0] = new DiscreteRGBLed(4, 27, 6, false);
-        _devices[1] = new ToshibaLED_I2C();
+    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
-        _devices[0] = new AP_BoardLED();
-        _devices[1] = new Buzzer();
-        _devices[2] = new Display();
+    ADD_BACKEND(new AP_BoardLED());
+    ADD_BACKEND(new Buzzer());
+    ADD_BACKEND(new Display());
 
   #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE
-        _devices[0] = new AP_BoardLED();
-        _devices[1] = new Display();
+    ADD_BACKEND(new AP_BoardLED());
+    ADD_BACKEND(new Display());
 
   #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
-        _devices[0] = new ToshibaLED_I2C();
-        _devices[1] = new ToneAlarm_Linux();
+    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
-        _devices[0] = new RCOutputRGBLedOff(15, 13, 14, 255);
+    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
-        _devices[0] = new AP_BoardLED();
+    ADD_BACKEND(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);
+    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
-        _devices[0] = new DiscoLED();
-        _devices[1] = new ToneAlarm_Linux();
+    ADD_BACKEND(new DiscoLED());
+    ADD_BACKEND(new ToneAlarm_Linux());
 
-    #else
-        _devices[0] = new AP_BoardLED();
-        _devices[1] = new ToshibaLED_I2C();
-        _devices[2] = 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
-    _devices[0] = new AP_BoardLED();
-    _devices[1] = new ToshibaLED_I2C();
+    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)
+{
+    // 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);
         }

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

libraries/AP_Notify/ToshibaLED_I2C.cpp

@@ -27,45 +27,47 @@
 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);
-    }
 
     // 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();
 
+    if (ret) {
         _dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&ToshibaLED_I2C::_timer, void));
+    }
 
     return ret;
 }

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

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)
 {