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