AP_Notify: add SITL Notify display

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),
 

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
 {

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;

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 <http://www.gnu.org/licenses/>.
+ */
+#ifdef WITH_SITL_OSD
+
+#include "Display_SITL.h"
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_Notify/AP_Notify.h>
+
+#include <stdio.h>
+#include <unistd.h>
+
+// 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; y<ROWS; y++) {
+                    for (uint16_t x=0; x<COLUMNS; x++) {
+                        if (buffer2[x+y/8*COLUMNS] & 1<<y%8) {
+                            image.setPixel(x, y, color_white);
+                        } else {
+                            image.setPixel(x, y, color_black);
+                        }
+                    }
+                }
+
+                sf::Texture texture;
+                texture.loadFromImage(image);
+                sf::Sprite sprite;
+                sprite.setTexture(texture, true);
+                sprite.setScale(SCALE, SCALE);
+                w->draw(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

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 <SFML/Graphics.h>
+#else
+#include <SFML/Graphics.hpp>
+#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