diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp
index 8e6fa15f7b..d101a32509 100644
--- a/libraries/AP_Notify/AP_Notify.cpp
+++ b/libraries/AP_Notify/AP_Notify.cpp
@@ -119,7 +119,7 @@ const AP_Param::GroupInfo AP_Notify::var_info[] = {
// @Param: DISPLAY_TYPE
// @DisplayName: Type of on-board I2C display
// @Description: This sets up the type of on-board I2C display. Disabled by default.
- // @Values: 0:Disable,1:ssd1306,2:sh1106
+ // @Values: 0:Disable,1:ssd1306,2:sh1106,10:SITL
// @User: Advanced
AP_GROUPINFO("DISPLAY_TYPE", 3, AP_Notify, _display_type, 0),
diff --git a/libraries/AP_Notify/AP_Notify.h b/libraries/AP_Notify/AP_Notify.h
index b7078302f1..6c7ab725d6 100644
--- a/libraries/AP_Notify/AP_Notify.h
+++ b/libraries/AP_Notify/AP_Notify.h
@@ -34,6 +34,7 @@
#define DISPLAY_OFF 0
#define DISPLAY_SSD1306 1
#define DISPLAY_SH1106 2
+#define DISPLAY_SITL 10
class AP_Notify
{
diff --git a/libraries/AP_Notify/Display.cpp b/libraries/AP_Notify/Display.cpp
index dae4c16612..6a341fd196 100644
--- a/libraries/AP_Notify/Display.cpp
+++ b/libraries/AP_Notify/Display.cpp
@@ -17,6 +17,7 @@
#include "Display.h"
#include "Display_SH1106_I2C.h"
#include "Display_SSD1306_I2C.h"
+#include "Display_SITL.h"
#include "AP_Notify.h"
@@ -333,6 +334,14 @@ bool Display::init(void)
_driver = Display_SH1106_I2C::probe(std::move(hal.i2c_mgr->get_device(i, NOTIFY_DISPLAY_I2C_ADDR)));
break;
}
+ case DISPLAY_SITL: {
+#ifdef WITH_SITL_OSD
+ _driver = Display_SITL::probe(); // never fails
+#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
+ ::fprintf(stderr, "SITL Display ineffective without --osd\n");
+#endif
+ break;
+ }
case DISPLAY_OFF:
default:
break;
diff --git a/libraries/AP_Notify/Display_SITL.cpp b/libraries/AP_Notify/Display_SITL.cpp
new file mode 100644
index 0000000000..61effccca7
--- /dev/null
+++ b/libraries/AP_Notify/Display_SITL.cpp
@@ -0,0 +1,159 @@
+/*
+ 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 .
+ */
+#ifdef WITH_SITL_OSD
+
+#include "Display_SITL.h"
+
+#include
+#include
+
+#include
+#include
+
+// constructor
+Display_SITL::Display_SITL()
+{
+}
+
+Display_SITL::~Display_SITL()
+{
+}
+
+Display_SITL *Display_SITL::probe()
+{
+ Display_SITL *driver = new Display_SITL();
+ if (!driver || !driver->hw_init()) {
+ delete driver;
+ return nullptr;
+ }
+ return driver;
+}
+
+// main loop of graphics thread
+void Display_SITL::update_thread(void)
+{
+ {
+ WITH_SEMAPHORE(AP::notify().sf_window_mutex);
+ w = new sf::RenderWindow(sf::VideoMode(COLUMNS*SCALE, ROWS*SCALE), "Display");
+ }
+ if (!w) {
+ AP_HAL::panic("Unable to create Display_SITL window");
+ }
+
+ const sf::Color color_black = sf::Color(0,0,0);
+ const sf::Color color_white = sf::Color(255,255,255);
+
+ const sf::Uint8 pixels[ROWS*COLUMNS*4]{};
+ sf::Image image;
+ image.create(COLUMNS, ROWS, pixels);
+
+ while (true) {
+ {
+ WITH_SEMAPHORE(AP::notify().sf_window_mutex);
+ sf::Event event;
+ while (w->pollEvent(event)) {
+ if (event.type == sf::Event::Closed) {
+ w->close();
+ }
+ }
+ if (!w->isOpen()) {
+ break;
+ }
+ if (_need_hw_update) {
+ _need_hw_update = false;
+
+ uint8_t buffer2[ROWS*COLUMNS];
+ {
+ WITH_SEMAPHORE(mutex);
+ memcpy(buffer2, _displaybuffer, sizeof(buffer2));
+ }
+ w->clear();
+
+ for (uint16_t y=0; ydraw(sprite);
+
+ w->display();
+ }
+ }
+ usleep(10000);
+ }
+}
+
+// trampoline for update thread
+void *Display_SITL::update_thread_start(void *obj)
+{
+ ((Display_SITL *)obj)->update_thread();
+ return nullptr;
+}
+
+bool Display_SITL::hw_init()
+{
+ pthread_create(&thread, NULL, update_thread_start, this);
+ _need_hw_update = true;
+
+ return true;
+}
+
+void Display_SITL::hw_update()
+{
+ _need_hw_update = true;
+}
+
+void Display_SITL::set_pixel(uint16_t x, uint16_t y)
+{
+ // check x, y range
+ if ((x >= COLUMNS) || (y >= ROWS)) {
+ return;
+ }
+ // set pixel in buffer
+ WITH_SEMAPHORE(mutex);
+ _displaybuffer[x + (y / 8 * COLUMNS)] |= 1 << (y % 8);
+ _need_hw_update = true;
+}
+
+void Display_SITL::clear_pixel(uint16_t x, uint16_t y)
+{
+ // check x, y range
+ if ((x >= COLUMNS) || (y >= ROWS)) {
+ return;
+ }
+ // clear pixel in buffer
+ WITH_SEMAPHORE(mutex);
+ _displaybuffer[x + (y / 8 * COLUMNS)] &= ~(1 << (y % 8));
+ _need_hw_update = true;
+}
+
+void Display_SITL::clear_screen()
+{
+ WITH_SEMAPHORE(mutex);
+ memset(_displaybuffer, 0, sizeof(_displaybuffer));
+ _need_hw_update = true;
+}
+
+#endif // WITH_SITL_OSD
diff --git a/libraries/AP_Notify/Display_SITL.h b/libraries/AP_Notify/Display_SITL.h
new file mode 100644
index 0000000000..25bc89fc18
--- /dev/null
+++ b/libraries/AP_Notify/Display_SITL.h
@@ -0,0 +1,51 @@
+#pragma once
+
+#ifdef WITH_SITL_OSD
+
+#include "Display.h"
+#include "Display_Backend.h"
+
+#ifdef HAVE_SFML_GRAPHICS_H
+#include
+#else
+#include
+#endif
+
+
+class Display_SITL: public Display_Backend {
+
+public:
+
+ static Display_SITL *probe();
+
+ 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_SITL();
+ ~Display_SITL() override;
+
+private:
+
+ static constexpr const uint16_t COLUMNS = 132;
+ static constexpr const uint8_t ROWS = 64;
+ static constexpr const uint8_t SCALE = 4; // make it more readable
+
+ bool hw_init() override;
+
+ void _timer();
+
+ uint8_t _displaybuffer[COLUMNS * ROWS];
+ bool _need_hw_update;
+
+ static void *update_thread_start(void *obj);
+ void update_thread(void);
+ sf::RenderWindow *w;
+ pthread_t thread;
+ HAL_Semaphore mutex;
+};
+
+#endif // WITH_SITL_OSD