AP_Notify: improve display detection

Create probe function for displays
Use a mask to determine I2C buses to probe

libraries/AP_Notify/Display.cpp

@@ -23,6 +23,8 @@
 #include <stdio.h>
 #include <AP_GPS/AP_GPS.h>
 
+#include <utility>
+
 extern const AP_HAL::HAL& hal;
 
 static const uint8_t _font[] = {
@@ -284,6 +286,9 @@ static const uint8_t _font[] = {
     0x00, 0x00, 0x00, 0x00, 0x00
 };
 
+// probe first 3 busses:
+static const uint8_t I2C_BUS_PROBE_MASK = 0x7;
+
 bool Display::init(void)
 {
     // exit immediately if already initialised
@@ -295,21 +300,26 @@ bool Display::init(void)
     _movedelay = 4; // ticker delay before shifting after new message displayed
 
     // initialise driver
-    switch (pNotify->_display_type) {
+    for(uint8_t i=0; i<8 && _driver == nullptr; i++) {
-        case DISPLAY_SSD1306:
+        if (! (I2C_BUS_PROBE_MASK & (1<<i))) {
-            _driver = new Display_SSD1306_I2C(hal.i2c_mgr->get_device(NOTIFY_DISPLAY_I2C_BUS, NOTIFY_DISPLAY_I2C_ADDR));
+            continue;
+        }
+        switch (pNotify->_display_type) {
+        case DISPLAY_SSD1306: {
+            _driver = Display_SSD1306_I2C::probe(std::move(hal.i2c_mgr->get_device(i, NOTIFY_DISPLAY_I2C_ADDR)));
             break;
-
+        }
-        case DISPLAY_SH1106:
+        case DISPLAY_SH1106: {
-            _driver = new Display_SH1106_I2C(hal.i2c_mgr->get_device(NOTIFY_DISPLAY_I2C_BUS, NOTIFY_DISPLAY_I2C_ADDR));
+            _driver = Display_SH1106_I2C::probe(std::move(hal.i2c_mgr->get_device(i, NOTIFY_DISPLAY_I2C_ADDR)));
             break;
-
+        }
         case DISPLAY_OFF:
         default:
             break;
+        }
     }
 
-    if (!_driver || !_driver->hw_init()) {
+    if (_driver == nullptr) {
         _healthy = false;
         return false;
     }

libraries/AP_Notify/Display_Backend.h

@@ -2,19 +2,21 @@
 
 #include "Display.h"
 
-#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI
-#define NOTIFY_DISPLAY_I2C_BUS 2
-#else
-#define NOTIFY_DISPLAY_I2C_BUS 1
-#endif
 #define NOTIFY_DISPLAY_I2C_ADDR 0x3C
 
 class Display_Backend {
 
 public:
-    virtual bool hw_init() = 0;
+
     virtual void hw_update() = 0;
     virtual void set_pixel(uint16_t x, uint16_t y) = 0;
     virtual void clear_pixel(uint16_t x, uint16_t y) = 0;
     virtual void clear_screen() = 0;
+
+protected:
+
+    virtual ~Display_Backend() {}
+
+    virtual bool hw_init() = 0;
+
 };

libraries/AP_Notify/Display_SH1106_I2C.cpp

@@ -28,6 +28,27 @@ Display_SH1106_I2C::Display_SH1106_I2C(AP_HAL::OwnPtr<AP_HAL::Device> 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)
+{
+    Display_SH1106_I2C *driver = new Display_SH1106_I2C(std::move(dev));
+    if (!driver || !driver->hw_init()) {
+        delete driver;
+        return nullptr;
+    }
+    return driver;
+}
+
+
 bool Display_SH1106_I2C::hw_init()
 {
     struct PACKED {

libraries/AP_Notify/Display_SH1106_I2C.h

@@ -11,19 +11,28 @@
 class Display_SH1106_I2C: public Display_Backend {
 
 public:
-    Display_SH1106_I2C(AP_HAL::OwnPtr<AP_HAL::Device> dev);
 
-    bool hw_init() override;
+    static Display_SH1106_I2C *probe(AP_HAL::OwnPtr<AP_HAL::Device> dev);
+
     void hw_update() override;
     void set_pixel(uint16_t x, uint16_t y) override;
     void clear_pixel(uint16_t x, uint16_t y) override;
     void clear_screen() override;
 
+protected:
+
+    Display_SH1106_I2C(AP_HAL::OwnPtr<AP_HAL::Device> dev);
+    ~Display_SH1106_I2C() override;
+
 private:
+
+    bool hw_init() override;
+
     void _timer();
 
     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

@@ -28,6 +28,27 @@ Display_SSD1306_I2C::Display_SSD1306_I2C(AP_HAL::OwnPtr<AP_HAL::Device> 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;
+}
+
+
+Display_SSD1306_I2C *Display_SSD1306_I2C::probe(AP_HAL::OwnPtr<AP_HAL::Device> dev)
+{
+    Display_SSD1306_I2C *driver = new Display_SSD1306_I2C(std::move(dev));
+    if (!driver || !driver->hw_init()) {
+        delete driver;
+        return nullptr;
+    }
+    return driver;
+}
+
 bool Display_SSD1306_I2C::hw_init()
 {
     struct PACKED {

libraries/AP_Notify/Display_SSD1306_I2C.h

@@ -11,15 +11,23 @@
 class Display_SSD1306_I2C: public Display_Backend {
 
 public:
-    Display_SSD1306_I2C(AP_HAL::OwnPtr<AP_HAL::Device> dev);
 
-    bool hw_init() override;
+    static Display_SSD1306_I2C *probe(AP_HAL::OwnPtr<AP_HAL::Device> dev);
+
     void hw_update() override;
     void set_pixel(uint16_t x, uint16_t y) override;
     void clear_pixel(uint16_t x, uint16_t y) override;
     void clear_screen() override;
 
+protected:
+
+    Display_SSD1306_I2C(AP_HAL::OwnPtr<AP_HAL::Device> dev);
+    ~Display_SSD1306_I2C() override;
+
 private:
+
+    bool hw_init() override;
+
     void _timer();
 
     AP_HAL::OwnPtr<AP_HAL::Device> _dev;