AP_Notify: remove semaphores protecting pixels

The semaphore-take-forevers were absolutely killing performance.

We can take some random pixel corruption to avoid vehicles crashing.

libraries/AP_Notify/Display_SH1106_I2C.cpp

@@ -25,17 +25,10 @@ static const AP_HAL::HAL& hal = AP_HAL::get_HAL();
 Display_SH1106_I2C::Display_SH1106_I2C(AP_HAL::OwnPtr<AP_HAL::Device> dev) :
     _dev(std::move(dev))
 {
-    _displaybuffer_sem = hal.util->new_semaphore();
 }
 
 Display_SH1106_I2C::~Display_SH1106_I2C()
 {
-    // note that a callback is registered below.  here we delete the
-    // semaphore, in that callback we use it.  That means - don't
-    // delete this Display backend if you've ever registered that
-    // callback!  This delete is only here to not leak memory during
-    // the detection phase.
-    delete _displaybuffer_sem;
 }
 
 Display_SH1106_I2C *Display_SH1106_I2C::probe(AP_HAL::OwnPtr<AP_HAL::Device> dev)
@@ -124,17 +117,10 @@ void Display_SH1106_I2C::_timer()
         command.page = 0xB0 | (i & 0x0F);
         _dev->transfer((uint8_t *)&command, sizeof(command), nullptr, 0);
 
-        if (_displaybuffer_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
             memcpy(&display_buffer.db[0], &_displaybuffer[i * SH1106_COLUMNS], SH1106_COLUMNS/2);
-            _displaybuffer_sem->give();
             _dev->transfer((uint8_t *)&display_buffer, SH1106_COLUMNS/2 + 1, nullptr, 0);
-        }
-
-        if (_displaybuffer_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
             memcpy(&display_buffer.db[0], &_displaybuffer[i * SH1106_COLUMNS + SH1106_COLUMNS/2 ], SH1106_COLUMNS/2);
-            _displaybuffer_sem->give();
             _dev->transfer((uint8_t *)&display_buffer, SH1106_COLUMNS/2 + 1, nullptr, 0);
-        }
     }
 }
 
@@ -145,11 +131,7 @@ void Display_SH1106_I2C::set_pixel(uint16_t x, uint16_t y)
         return;
     }
     // set pixel in buffer
-    if (!_displaybuffer_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
-        return;
-    }
     _displaybuffer[x + (y / 8 * SH1106_COLUMNS)] |= 1 << (y % 8);
-    _displaybuffer_sem->give();
 }
 
 void Display_SH1106_I2C::clear_pixel(uint16_t x, uint16_t y)
@@ -158,19 +140,11 @@ void Display_SH1106_I2C::clear_pixel(uint16_t x, uint16_t y)
     if ((x >= SH1106_COLUMNS) || (y >= SH1106_ROWS)) {
         return;
     }
-    if (!_displaybuffer_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
-        return;
-    }
     // clear pixel in buffer
     _displaybuffer[x + (y / 8 * SH1106_COLUMNS)] &= ~(1 << (y % 8));
-    _displaybuffer_sem->give();
 }
 
 void Display_SH1106_I2C::clear_screen()
 {
-    if (!_displaybuffer_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
-        return;
-    }
     memset(_displaybuffer, 0, SH1106_COLUMNS * SH1106_ROWS_PER_PAGE);
-    _displaybuffer_sem->give();
 }

libraries/AP_Notify/Display_SH1106_I2C.h

@@ -32,7 +32,6 @@ private:
 
     AP_HAL::OwnPtr<AP_HAL::Device> _dev;
     uint8_t _displaybuffer[SH1106_COLUMNS * SH1106_ROWS_PER_PAGE];
-    AP_HAL::Semaphore *_displaybuffer_sem;
     bool _need_hw_update;
 
 };

libraries/AP_Notify/Display_SSD1306_I2C.cpp

@@ -25,17 +25,10 @@ static const AP_HAL::HAL& hal = AP_HAL::get_HAL();
 Display_SSD1306_I2C::Display_SSD1306_I2C(AP_HAL::OwnPtr<AP_HAL::Device> dev) :
     _dev(std::move(dev))
 {
-    _displaybuffer_sem = hal.util->new_semaphore();
 }
 
 Display_SSD1306_I2C::~Display_SSD1306_I2C()
 {
-    // note that a callback is registered below.  here we delete the
-    // semaphore, in that callback we use it.  That means - don't
-    // delete this Display backend if you've ever registered that
-    // callback!  This delete is only here to not leak memory during
-    // the detection phase.
-    delete _displaybuffer_sem;
 }
 
 
@@ -130,17 +123,11 @@ void Display_SSD1306_I2C::_timer()
         command.cmd[4] = i;
         _dev->transfer((uint8_t *)&command, sizeof(command), nullptr, 0);
 
-        if (_displaybuffer_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
             memcpy(&display_buffer.db[0], &_displaybuffer[i * SSD1306_COLUMNS], SSD1306_COLUMNS/2);
-            _displaybuffer_sem->give();
             _dev->transfer((uint8_t *)&display_buffer, SSD1306_COLUMNS/2 + 1, nullptr, 0);
-        }
 
-        if (_displaybuffer_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
             memcpy(&display_buffer.db[0], &_displaybuffer[i * SSD1306_COLUMNS + SSD1306_COLUMNS/2 ], SSD1306_COLUMNS/2);
-            _displaybuffer_sem->give();
             _dev->transfer((uint8_t *)&display_buffer, SSD1306_COLUMNS/2 + 1, nullptr, 0);
-        }
     }
 }
 
@@ -150,12 +137,8 @@ void Display_SSD1306_I2C::set_pixel(uint16_t x, uint16_t y)
     if ((x >= SSD1306_COLUMNS) || (y >= SSD1306_ROWS)) {
         return;
     }
-    if (!_displaybuffer_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
-        return;
-    }
     // set pixel in buffer
     _displaybuffer[x + (y / 8 * SSD1306_COLUMNS)] |= 1 << (y % 8);
-    _displaybuffer_sem->give();
 }
 
 void Display_SSD1306_I2C::clear_pixel(uint16_t x, uint16_t y)
@@ -164,19 +147,11 @@ void Display_SSD1306_I2C::clear_pixel(uint16_t x, uint16_t y)
     if ((x >= SSD1306_COLUMNS) || (y >= SSD1306_ROWS)) {
         return;
     }
-    if (!_displaybuffer_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
-        return;
-    }
     // clear pixel in buffer
     _displaybuffer[x + (y / 8 * SSD1306_COLUMNS)] &= ~(1 << (y % 8));
-    _displaybuffer_sem->give();
 }
 
 void Display_SSD1306_I2C::clear_screen()
 {
-    if (!_displaybuffer_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
-        return;
-    }
      memset(_displaybuffer, 0, SSD1306_COLUMNS * SSD1306_ROWS_PER_PAGE);
-     _displaybuffer_sem->give();
 }

libraries/AP_Notify/Display_SSD1306_I2C.h

@@ -32,6 +32,5 @@ private:
 
     AP_HAL::OwnPtr<AP_HAL::Device> _dev;
     uint8_t _displaybuffer[SSD1306_COLUMNS * SSD1306_ROWS_PER_PAGE];
-    AP_HAL::Semaphore *_displaybuffer_sem;
     bool _need_hw_update;
 };