From bcb0ead71a42f3705304a9854c26e164e3ef52c4 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 11 Mar 2017 12:16:55 +1100 Subject: [PATCH] AP_Notify: improve display detection Create probe function for displays Use a mask to determine I2C buses to probe --- libraries/AP_Notify/Display.cpp | 26 ++++++++++++++------- libraries/AP_Notify/Display_Backend.h | 14 ++++++----- libraries/AP_Notify/Display_SH1106_I2C.cpp | 21 +++++++++++++++++ libraries/AP_Notify/Display_SH1106_I2C.h | 13 +++++++++-- libraries/AP_Notify/Display_SSD1306_I2C.cpp | 21 +++++++++++++++++ libraries/AP_Notify/Display_SSD1306_I2C.h | 12 ++++++++-- 6 files changed, 89 insertions(+), 18 deletions(-) diff --git a/libraries/AP_Notify/Display.cpp b/libraries/AP_Notify/Display.cpp index 3bcfa1068d..0fa1095f6d 100644 --- a/libraries/AP_Notify/Display.cpp +++ b/libraries/AP_Notify/Display.cpp @@ -23,6 +23,8 @@ #include #include +#include + 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) { - case DISPLAY_SSD1306: - _driver = new Display_SSD1306_I2C(hal.i2c_mgr->get_device(NOTIFY_DISPLAY_I2C_BUS, NOTIFY_DISPLAY_I2C_ADDR)); + for(uint8_t i=0; i<8 && _driver == nullptr; i++) { + if (! (I2C_BUS_PROBE_MASK & (1<_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: - _driver = new Display_SH1106_I2C(hal.i2c_mgr->get_device(NOTIFY_DISPLAY_I2C_BUS, NOTIFY_DISPLAY_I2C_ADDR)); + } + case DISPLAY_SH1106: { + _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; } diff --git a/libraries/AP_Notify/Display_Backend.h b/libraries/AP_Notify/Display_Backend.h index 06d06d6548..689ecbd182 100644 --- a/libraries/AP_Notify/Display_Backend.h +++ b/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; + }; diff --git a/libraries/AP_Notify/Display_SH1106_I2C.cpp b/libraries/AP_Notify/Display_SH1106_I2C.cpp index 5552bbab46..e069222f12 100644 --- a/libraries/AP_Notify/Display_SH1106_I2C.cpp +++ b/libraries/AP_Notify/Display_SH1106_I2C.cpp @@ -28,6 +28,27 @@ Display_SH1106_I2C::Display_SH1106_I2C(AP_HAL::OwnPtr 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 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 { diff --git a/libraries/AP_Notify/Display_SH1106_I2C.h b/libraries/AP_Notify/Display_SH1106_I2C.h index b9c6403190..fe7b512bec 100644 --- a/libraries/AP_Notify/Display_SH1106_I2C.h +++ b/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 dev); - bool hw_init() override; + static Display_SH1106_I2C *probe(AP_HAL::OwnPtr 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 dev); + ~Display_SH1106_I2C() override; + private: + + bool hw_init() override; + void _timer(); AP_HAL::OwnPtr _dev; uint8_t _displaybuffer[SH1106_COLUMNS * SH1106_ROWS_PER_PAGE]; AP_HAL::Semaphore *_displaybuffer_sem; bool _need_hw_update; + }; diff --git a/libraries/AP_Notify/Display_SSD1306_I2C.cpp b/libraries/AP_Notify/Display_SSD1306_I2C.cpp index 9624eb1438..c2670ca163 100644 --- a/libraries/AP_Notify/Display_SSD1306_I2C.cpp +++ b/libraries/AP_Notify/Display_SSD1306_I2C.cpp @@ -28,6 +28,27 @@ Display_SSD1306_I2C::Display_SSD1306_I2C(AP_HAL::OwnPtr 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 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 { diff --git a/libraries/AP_Notify/Display_SSD1306_I2C.h b/libraries/AP_Notify/Display_SSD1306_I2C.h index 8ac971014c..725ba5a51f 100644 --- a/libraries/AP_Notify/Display_SSD1306_I2C.h +++ b/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 dev); - bool hw_init() override; + static Display_SSD1306_I2C *probe(AP_HAL::OwnPtr 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 dev); + ~Display_SSD1306_I2C() override; + private: + + bool hw_init() override; + void _timer(); AP_HAL::OwnPtr _dev;