diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp
index ccb746c31a..3647123daa 100644
--- a/libraries/AP_Notify/AP_Notify.cpp
+++ b/libraries/AP_Notify/AP_Notify.cpp
@@ -17,7 +17,7 @@
#include "AP_BoardLED.h"
#include "Buzzer.h"
-#include "Display_OLED_I2C.h"
+#include "Display.h"
#include "ExternalLED.h"
#include "NavioLED_I2C.h"
#include "OreoLED_PX4.h"
@@ -82,7 +82,7 @@ char AP_Notify::_send_text[NOTIFY_TEXT_BUFFER_SIZE] {};
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
AP_BoardLED boardled;
ToshibaLED_PX4 toshibaled;
- Display_OLED_I2C display;
+ Display display;
#if AP_NOTIFY_SOLO_TONES == 1
ToneAlarm_PX4_Solo tonealarm;
@@ -119,7 +119,7 @@ char AP_Notify::_send_text[NOTIFY_TEXT_BUFFER_SIZE] {};
NotifyDevice *AP_Notify::_devices[] = {&navioled, &toshibaled};
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI
Buzzer buzzer;
- Display_OLED_I2C display;
+ Display display;
NotifyDevice *AP_Notify::_devices[] = {&display, &buzzer};
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
ToshibaLED_I2C toshibaled;
diff --git a/libraries/AP_Notify/AP_Notify.h b/libraries/AP_Notify/AP_Notify.h
index eb5f637726..edc12a233b 100644
--- a/libraries/AP_Notify/AP_Notify.h
+++ b/libraries/AP_Notify/AP_Notify.h
@@ -47,7 +47,7 @@
class AP_Notify
{
friend class RGBLed; // RGBLed needs access to notify parameters
- friend class Display_OLED_I2C; // Display_OLED_I2C needs access to notify parameters
+ friend class Display; // Display needs access to notify parameters
public:
// Constructor
AP_Notify();
diff --git a/libraries/AP_Notify/Display.cpp b/libraries/AP_Notify/Display.cpp
index 93afae0126..abbf8be1be 100644
--- a/libraries/AP_Notify/Display.cpp
+++ b/libraries/AP_Notify/Display.cpp
@@ -15,6 +15,8 @@
/* Notify display driver for 128 x 64 pixel displays */
#include "Display.h"
+#include "Display_SH1106_I2C.h"
+#include "Display_SSD1306_I2C.h"
#include "AP_Notify.h"
@@ -354,10 +356,32 @@ static const char * _modename[] = {
bool Display::init(void)
{
+ // exit immediately if already initialised
+ if (_driver != nullptr) {
+ return true;
+ }
+
_mstartpos = 0; // ticker shift position
_movedelay = 4; // ticker delay before shifting after new message displayed
- _healthy = hw_init();
+ // initialise driver
+ switch (pNotify->_display_type) {
+ case DISPLAY_SSD1306:
+ _driver = new Display_SSD1306_I2C();
+ break;
+
+ case DISPLAY_SH1106:
+ _driver = new Display_SH1106_I2C();
+ break;
+
+ case DISPLAY_OFF:
+ default:
+ break;
+ }
+
+ if (_driver != nullptr) {
+ _healthy = _driver->hw_init();
+ }
if (!_healthy) {
return false;
@@ -365,7 +389,7 @@ bool Display::init(void)
// update all on display
update_all();
- hw_update();
+ _driver->hw_update();
return true;
}
@@ -389,22 +413,21 @@ void Display::update()
if (AP_Notify::flags.armed) {
if (screenpage != 1) {
- clear_screen();
+ _driver->clear_screen();
update_arm(3);
screenpage = 1;
- hw_update(); //update hw once , do not transmition to display in fly
+ _driver->hw_update(); //update hw once , do not transmition to display in fly
}
return;
}
-
if (screenpage != 2) {
- clear_screen(); //once clear screen when page changed
+ _driver->clear_screen(); //once clear screen when page changed
screenpage = 2;
}
update_all();
- hw_update(); //update at 2 Hz in disarmed mode
+ _driver->hw_update(); //update at 2 Hz in disarmed mode
}
@@ -446,9 +469,9 @@ void Display::draw_char(uint16_t x, uint16_t y, const char c)
for (uint8_t j = 0; j < 8; j++) {
if (line & 1) {
- set_pixel(x + i, y + j);
+ _driver->set_pixel(x + i, y + j);
} else {
- clear_pixel(x + i, y + j);
+ _driver->clear_pixel(x + i, y + j);
}
line >>= 1;
}
diff --git a/libraries/AP_Notify/Display.h b/libraries/AP_Notify/Display.h
index 86cef6cb1f..950eabb07f 100644
--- a/libraries/AP_Notify/Display.h
+++ b/libraries/AP_Notify/Display.h
@@ -7,18 +7,15 @@
#define DISPLAY_MESSAGE_SIZE 18
+class Display_Backend;
+
class Display: public NotifyDevice {
public:
+ friend class Display_Backend;
+
bool init(void);
void update();
-protected:
- virtual bool hw_init() = 0;
- virtual bool hw_update() = 0;
- virtual bool set_pixel(uint16_t x, uint16_t y) = 0;
- virtual bool clear_pixel(uint16_t x, uint16_t y) = 0;
- virtual bool clear_screen() = 0;
-
private:
void draw_char(uint16_t x, uint16_t y, const char c);
void draw_text(uint16_t x, uint16_t y, const char *c);
@@ -32,6 +29,8 @@ private:
void update_mode(uint8_t r);
void update_text(uint8_t r);
+ Display_Backend *_driver;
+
bool _healthy;
uint8_t _mstartpos;
diff --git a/libraries/AP_Notify/Display_Backend.h b/libraries/AP_Notify/Display_Backend.h
new file mode 100644
index 0000000000..26fa5366ff
--- /dev/null
+++ b/libraries/AP_Notify/Display_Backend.h
@@ -0,0 +1,19 @@
+#pragma once
+
+#include "Display.h"
+
+#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI
+#define OLED_I2C_BUS 2
+#else
+#define OLED_I2C_BUS 1
+#endif
+
+class Display_Backend {
+
+public:
+ virtual bool hw_init() = 0;
+ virtual bool hw_update() = 0;
+ virtual bool set_pixel(uint16_t x, uint16_t y) = 0;
+ virtual bool clear_pixel(uint16_t x, uint16_t y) = 0;
+ virtual bool clear_screen() = 0;
+};
diff --git a/libraries/AP_Notify/Display_OLED_I2C.cpp b/libraries/AP_Notify/Display_OLED_I2C.cpp
deleted file mode 100644
index 6bb40cad6c..0000000000
--- a/libraries/AP_Notify/Display_OLED_I2C.cpp
+++ /dev/null
@@ -1,43 +0,0 @@
-/*
- This program is free software: you can redistribute it and/or modify
- it under the terms of the GNU General Public License as published by
- the Free Software Foundation, either version 3 of the License, or
- (at your option) any later version.
-
- This program is distributed in the hope that it will be useful,
- but WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- GNU General Public License for more details.
-
- You should have received a copy of the GNU General Public License
- along with this program. If not, see .
- */
-#include "Display_OLED_I2C.h"
-#include "Display_SH1106_I2C.h"
-#include "Display_SSD1306_I2C.h"
-
-#include
-
-#include
-#include
-#include "AP_Notify.h"
-
-Display_OLED_I2C* Display_OLED_I2C::getInstance()
-{
- if (nullptr == _instance.get()) {
- switch (pNotify->_display_type) {
- case DISPLAY_SSD1306:
- _instance = new Display_SSD1306_I2C();
- break;
-
- case DISPLAY_SH1106:
- _instance = new Display_SH1106_I2C();
- break;
-
- case DISPLAY_OFF:
- default:
- break;
- }
- }
- return _instance.get();
-}
\ No newline at end of file
diff --git a/libraries/AP_Notify/Display_OLED_I2C.h b/libraries/AP_Notify/Display_OLED_I2C.h
deleted file mode 100644
index fc564ea886..0000000000
--- a/libraries/AP_Notify/Display_OLED_I2C.h
+++ /dev/null
@@ -1,27 +0,0 @@
-#pragma once
-
-#include "Display.h"
-
-#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI
-#define OLED_I2C_BUS 2
-#else
-#define OLED_I2C_BUS 1
-#endif
-
-class Display_OLED_I2C: public Display {
-
-protected:
- virtual bool hw_init() { return hasInstance() && getInstance()->hw_init(); }
- virtual bool hw_update() { return hasInstance() && getInstance()->hw_update(); }
- virtual bool set_pixel(uint16_t x, uint16_t y) { return hasInstance() && getInstance()->set_pixel(x, y); }
- virtual bool clear_pixel(uint16_t x, uint16_t y) { return hasInstance() && getInstance()->clear_pixel(x, y); }
- virtual bool clear_screen() { return hasInstance() && getInstance()->clear_screen(); }
-
- virtual void _update_timer() {};
-
-private:
- bool hasInstance() { return getInstance() != nullptr; }
-
- AP_HAL::OwnPtr _instance;
- Display_OLED_I2C* getInstance();
-};
diff --git a/libraries/AP_Notify/Display_SH1106_I2C.cpp b/libraries/AP_Notify/Display_SH1106_I2C.cpp
index 071df8a1d0..0eff2e7046 100644
--- a/libraries/AP_Notify/Display_SH1106_I2C.cpp
+++ b/libraries/AP_Notify/Display_SH1106_I2C.cpp
@@ -57,16 +57,17 @@ bool Display_SH1106_I2C::hw_init()
}
// init display
- _dev->transfer((uint8_t *)&init_seq, sizeof(init_seq), nullptr, 0);
+ bool success = _dev->transfer((uint8_t *)&init_seq, sizeof(init_seq), nullptr, 0);
// give back i2c semaphore
_dev->get_semaphore()->give();
- _need_hw_update = true;
+ if (success) {
+ _need_hw_update = true;
+ _dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&Display_SH1106_I2C::_timer, void));
+ }
- _dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&Display_SH1106_I2C::_update_timer, void));
-
- return true;
+ return success;
}
bool Display_SH1106_I2C::hw_update()
@@ -75,7 +76,7 @@ bool Display_SH1106_I2C::hw_update()
return true;
}
-void Display_SH1106_I2C::_update_timer()
+void Display_SH1106_I2C::_timer()
{
if (!_need_hw_update) {
return;
diff --git a/libraries/AP_Notify/Display_SH1106_I2C.h b/libraries/AP_Notify/Display_SH1106_I2C.h
index 7a342bb316..2247c4fc12 100644
--- a/libraries/AP_Notify/Display_SH1106_I2C.h
+++ b/libraries/AP_Notify/Display_SH1106_I2C.h
@@ -1,25 +1,25 @@
#pragma once
#include "Display.h"
-#include "Display_OLED_I2C.h"
+#include "Display_Backend.h"
#include
#define SH1106_COLUMNS 132 // display columns
#define SH1106_ROWS 64 // display rows
#define SH1106_ROWS_PER_PAGE 8
-class Display_SH1106_I2C: public Display_OLED_I2C {
+class Display_SH1106_I2C: public Display_Backend {
-protected:
+public:
bool hw_init() override;
bool hw_update() override;
bool set_pixel(uint16_t x, uint16_t y) override;
bool clear_pixel(uint16_t x, uint16_t y) override;
bool clear_screen() override;
- void _update_timer() override;
-
private:
+ void _timer();
+
AP_HAL::OwnPtr _dev;
uint8_t _displaybuffer[SH1106_COLUMNS * SH1106_ROWS_PER_PAGE];
bool _need_hw_update;
diff --git a/libraries/AP_Notify/Display_SSD1306_I2C.cpp b/libraries/AP_Notify/Display_SSD1306_I2C.cpp
index f57e49d477..bdc877fc9e 100644
--- a/libraries/AP_Notify/Display_SSD1306_I2C.cpp
+++ b/libraries/AP_Notify/Display_SSD1306_I2C.cpp
@@ -66,16 +66,17 @@ bool Display_SSD1306_I2C::hw_init()
}
// init display
- _dev->transfer((uint8_t *)&init_seq, sizeof(init_seq), nullptr, 0);
+ bool success = _dev->transfer((uint8_t *)&init_seq, sizeof(init_seq), nullptr, 0);
// give back i2c semaphore
_dev->get_semaphore()->give();
- _need_hw_update = true;
+ if (success) {
+ _need_hw_update = true;
+ _dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&Display_SSD1306_I2C::_timer, void));
+ }
- _dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&Display_SSD1306_I2C::_update_timer, void));
-
- return true;
+ return success;
}
bool Display_SSD1306_I2C::hw_update()
@@ -84,7 +85,7 @@ bool Display_SSD1306_I2C::hw_update()
return true;
}
-void Display_SSD1306_I2C::_update_timer()
+void Display_SSD1306_I2C::_timer()
{
if (!_need_hw_update) {
return;
diff --git a/libraries/AP_Notify/Display_SSD1306_I2C.h b/libraries/AP_Notify/Display_SSD1306_I2C.h
index 22cad641d0..a363e03b42 100644
--- a/libraries/AP_Notify/Display_SSD1306_I2C.h
+++ b/libraries/AP_Notify/Display_SSD1306_I2C.h
@@ -1,25 +1,25 @@
#pragma once
#include "Display.h"
-#include "Display_OLED_I2C.h"
+#include "Display_Backend.h"
#include
#define SSD1306_COLUMNS 128 // display columns
#define SSD1306_ROWS 64 // display rows
#define SSD1306_ROWS_PER_PAGE 8
-class Display_SSD1306_I2C: public Display_OLED_I2C {
+class Display_SSD1306_I2C: public Display_Backend {
-protected:
+public:
bool hw_init() override;
bool hw_update() override;
bool set_pixel(uint16_t x, uint16_t y) override;
bool clear_pixel(uint16_t x, uint16_t y) override;
bool clear_screen() override;
- void _update_timer() override;
-
private:
+ void _timer();
+
AP_HAL::OwnPtr _dev;
uint8_t _displaybuffer[SSD1306_COLUMNS * SSD1306_ROWS_PER_PAGE];
bool _need_hw_update;