AP_Notify: fixed threading in two notify backends

libraries/AP_Notify/Display_SSD1306_I2C.cpp

@@ -49,14 +49,26 @@ bool Display_SSD1306_I2C::hw_init()
     // give back i2c semaphore
     _dev->get_semaphore()->give();
 
-    // clear display
-    hw_update();
+    _need_hw_update = true;
+
+    _dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&Display_SSD1306_I2C::_timer, bool));
 
     return true;
 }
 
 bool Display_SSD1306_I2C::hw_update()
 {
+    _need_hw_update = true;
+    return true;
+}
+
+bool Display_SSD1306_I2C::_timer()
+{
+    if (!_need_hw_update) {
+        return true;
+    }
+    _need_hw_update = false;
+    
     struct PACKED {
         uint8_t reg;
         uint8_t cmd[6];
@@ -67,10 +79,6 @@ bool Display_SSD1306_I2C::hw_update()
         uint8_t db[SSD1306_ROWS];
     } display_buffer = { 0x40, {} };
 
-    if (!_dev || !_dev->get_semaphore()->take(5)) {
-        return false;
-    }
-
     // write buffer to display
     for (uint8_t i = 0; i < (SSD1306_COLUMNS / SSD1306_COLUMNS_PER_PAGE); i++) {
         command.cmd[4] = i;
@@ -81,9 +89,6 @@ bool Display_SSD1306_I2C::hw_update()
         _dev->transfer((uint8_t *)&display_buffer, sizeof(display_buffer), nullptr, 0);
     }
 
-    // give back i2c semaphore
-    _dev->get_semaphore()->give();
-
     return true;
 }
 

libraries/AP_Notify/Display_SSD1306_I2C.h

@@ -17,4 +17,6 @@ public:
 private:
     AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
     uint8_t _displaybuffer[SSD1306_ROWS * SSD1306_COLUMNS_PER_PAGE];
+    bool _need_hw_update;
+    bool _timer(void);
 };

libraries/AP_Notify/NavioLED_I2C.cpp

@@ -28,31 +28,35 @@ bool NavioLED_I2C::hw_init()
 {
     _dev = hal.i2c_mgr->get_device(1, PCA9685_ADDRESS);
 
-    // take i2c bus sempahore
-    if (!_dev || !_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
+    if (!_dev) {
         return false;
     }
 
-    // enable the led
-    bool ret = true;
+    _dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&NavioLED_I2C::_timer, bool));
 
-    // give back i2c semaphore
-    _dev->get_semaphore()->give();
-
-    return ret;
+    return true;
 }
 
 // set_rgb - set color as a combination of red, green and blue values
 bool NavioLED_I2C::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
 {
-    // exit immediately if we can't take the semaphore
-    if (!_dev || !_dev->get_semaphore()->take(5)) {
-        return false;
-    }
+    rgb.r = red;
+    rgb.g = green;
+    rgb.b = blue;
+    _need_update = true;
+    return true;
+}
 
-    uint16_t red_adjusted = red * 0x10;
-    uint16_t green_adjusted = green * 0x10;
-    uint16_t blue_adjusted = blue * 0x10;
+bool NavioLED_I2C::_timer(void)
+{
+    if (!_need_update) {
+        return true;
+    }
+    _need_update = false;
+    
+    uint16_t red_adjusted = rgb.r * 0x10;
+    uint16_t green_adjusted = rgb.g * 0x10;
+    uint16_t blue_adjusted = rgb.b * 0x10;
 
     uint8_t blue_channel_lsb = blue_adjusted & 0xFF;
     uint8_t blue_channel_msb = blue_adjusted >> 8;
@@ -68,9 +72,7 @@ bool NavioLED_I2C::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
 			     0x00, 0x00, green_channel_lsb, green_channel_msb,
 			     0x00, 0x00, red_channel_lsb, red_channel_msb};
 
-    bool success = _dev->transfer(transaction, sizeof(transaction), nullptr, 0);
+    _dev->transfer(transaction, sizeof(transaction), nullptr, 0);
     
-    // give back i2c semaphore
-    _dev->get_semaphore()->give();
-    return success;
+    return true;
 }

libraries/AP_Notify/NavioLED_I2C.h

@@ -27,4 +27,9 @@ protected:
 
 private:
     AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
+    bool _timer(void);
+    struct {
+        uint8_t r, g, b;
+    } rgb;
+    bool _need_update;
 };