AP_Notify: Display_SSD1306_I2C: use I2CDevice interface

libraries/AP_Notify/Display_SSD1306_I2C.cpp

@@ -20,35 +20,32 @@
 
 static const AP_HAL::HAL& hal = AP_HAL::get_HAL();
 
+Display_SSD1306_I2C::Display_SSD1306_I2C():
+    _dev(hal.i2c_mgr->get_device(2, SSD1306_I2C_ADDRESS))
+{
+}
+
 bool Display_SSD1306_I2C::hw_init()
 {
-    // display init sequence
-    uint8_t init_seq[] = {0xAE, 0xD5, 0x80, 0xA8, 0x3F, 0xD3,
+    struct {
+        uint8_t reg;
+        uint8_t seq[31];
+    } init_seq = { 0x0,  {0xAE, 0xD5, 0x80, 0xA8, 0x3F, 0xD3,
                           0x00, 0x40, 0x8D, 0x14, 0x20, 0x00,
                           0xA1, 0xC8, 0xDA, 0x12, 0x81, 0xCF,
                           0xD9, 0xF1, 0xDB, 0x40, 0xA4, 0xA6,
-                          0xAF, 0x21, 0, 127, 0x22, 0, 7
-                         };
-
-    // get pointer to i2c bus semaphore
-    AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();
+                          0xAF, 0x21, 0, 127, 0x22, 0, 7 } };
 
     // 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);
-
     // init display
-    hal.i2c->writeRegisters(SSD1306_I2C_ADDRESS, 0x0, sizeof(init_seq), init_seq);
-
-    // re-enable recording of i2c lockup errors
-    hal.i2c->ignore_errors(false);
+    _dev->transfer((uint8_t *)&init_seq, sizeof(init_seq), nullptr, 0);
 
     // give back i2c semaphore
-    i2c_sem->give();
+    _dev->get_semaphore()->give();
 
     // clear display
     hw_update();
@@ -58,25 +55,28 @@ bool Display_SSD1306_I2C::hw_init()
 
 bool Display_SSD1306_I2C::hw_update()
 {
-    uint8_t command[] = {0x21, 0, 127, 0x22, 0, 7};
+    struct {
+        uint8_t reg;
+        uint8_t cmd[6];
+    } command = { 0x0, {0x21, 0, 127, 0x22, 0, 7} };
 
-    // 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;
-    }
+    struct {
+        uint8_t reg;
+        uint8_t db[SSD1306_ROWS];
+    } display_buffer = { 0x40 };
 
     // write buffer to display
     for (uint8_t i = 0; i < (SSD1306_COLUMNS / SSD1306_COLUMNS_PER_PAGE); i++) {
-        command[4] = i;
-        hal.i2c->writeRegisters(SSD1306_I2C_ADDRESS, 0x0, sizeof(command), command);
-        hal.i2c->writeRegisters(SSD1306_I2C_ADDRESS, 0x40, SSD1306_ROWS, &_displaybuffer[i * SSD1306_ROWS]);
+        command.cmd[4] = i;
+
+        _dev->transfer((uint8_t *)&command, sizeof(command), nullptr, 0);
+
+        memcpy(&display_buffer.db[0], &_displaybuffer[i * SSD1306_ROWS], SSD1306_ROWS);
+        _dev->transfer((uint8_t *)&display_buffer, sizeof(display_buffer), nullptr, 0);
     }
 
     // give back i2c semaphore
-    i2c_sem->give();
+    _dev->get_semaphore()->give();
 
     return true;
 }

libraries/AP_Notify/Display_SSD1306_I2C.h

@@ -2,6 +2,7 @@
 #pragma once
 
 #include "Display.h"
+#include <AP_HAL/I2CDevice.h>
 
 #define SSD1306_I2C_ADDRESS 0x3C    // default I2C bus address
 #define SSD1306_ROWS 128		    // display rows
@@ -10,11 +11,13 @@
 
 class Display_SSD1306_I2C: public Display {
 public:
+    Display_SSD1306_I2C();
     virtual bool hw_init();
     virtual bool hw_update();
     virtual bool set_pixel(uint16_t x, uint16_t y);
     virtual bool clear_pixel(uint16_t x, uint16_t y);
 
 private:
+    AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
     uint8_t _displaybuffer[SSD1306_ROWS * SSD1306_COLUMNS_PER_PAGE];
 };