diff --git a/libraries/AP_OSD/AP_OSD.cpp b/libraries/AP_OSD/AP_OSD.cpp index 0d6cd7a1dd..de64ee1ee2 100644 --- a/libraries/AP_OSD/AP_OSD.cpp +++ b/libraries/AP_OSD/AP_OSD.cpp @@ -19,6 +19,9 @@ #include "AP_OSD.h" #include "AP_OSD_MAX7456.h" +#ifdef WITH_SITL_OSD +#include "AP_OSD_SITL.h" +#endif #include #include #include @@ -86,7 +89,7 @@ void AP_OSD::init() default: break; - case OSD_MAX7456: + case OSD_MAX7456: { AP_HAL::OwnPtr spi_dev = std::move(hal.spi->get_device("osd")); if (!spi_dev) { break; @@ -99,6 +102,19 @@ void AP_OSD::init() hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&AP_OSD::timer, void)); break; } + +#ifdef WITH_SITL_OSD + case OSD_SITL: { + backend = AP_OSD_SITL::probe(*this); + if (backend == nullptr) { + break; + } + hal.console->printf("Started SITL OSD\n"); + hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&AP_OSD::timer, void)); + break; + } +#endif + } } void AP_OSD::timer() diff --git a/libraries/AP_OSD/AP_OSD.h b/libraries/AP_OSD/AP_OSD.h index fea19ad87f..aad1408592 100644 --- a/libraries/AP_OSD/AP_OSD.h +++ b/libraries/AP_OSD/AP_OSD.h @@ -117,6 +117,7 @@ public: enum osd_types { OSD_NONE=0, OSD_MAX7456=1, + OSD_SITL=2, }; enum switch_method { TOGGLE=0, diff --git a/libraries/AP_OSD/AP_OSD_SITL.cpp b/libraries/AP_OSD/AP_OSD_SITL.cpp new file mode 100644 index 0000000000..7dcb876db9 --- /dev/null +++ b/libraries/AP_OSD/AP_OSD_SITL.cpp @@ -0,0 +1,157 @@ +/* + * This file 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 file 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 . + * + */ +/* + OSD backend for SITL. Uses SFML media library. See + https://www.sfml-dev.org/index.php + + To use install SFML libraries, and run sim_vehicle.py with --osd + option. Then set OSD_TYPE to 2 + */ +#ifdef WITH_SITL_OSD + +#include "AP_OSD_SITL.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +/* + load *.bin font file, in MAX7456 format + */ +void AP_OSD_SITL::load_font(void) +{ + uint32_t font_size; + const uint8_t *font_data = AP_ROMFS::find_file("osd_font.bin", font_size); + if (font_data == nullptr || font_size != 54 * 256) { + AP_HAL::panic("Bad font file"); + } + for (uint16_t i=0; i<256; i++) { + const uint8_t *c = &font_data[i*54]; + // each pixel is 4 bytes, RGBA + sf::Uint8 *pixels = new sf::Uint8[char_width * char_height * 4]; + if (!font[i].create(char_width, char_height)) { + AP_HAL::panic("Failed to create texture"); + } + for (uint16_t y=0; y> bitshift) & 3; + sf::Uint8 *p = &pixels[(y*char_width+x)*4]; + switch (v) { + case 0: + p[0] = 0; + p[1] = 0; + p[2] = 0; + p[3] = 255; + break; + case 1: + case 3: + p[0] = 0; + p[1] = 0; + p[2] = 0; + p[3] = 0; + break; + case 2: + p[0] = 255; + p[1] = 255; + p[2] = 255; + p[3] = 255; + break; + } + + } + } + font[i].update(pixels); + } +} + +void AP_OSD_SITL::write(uint8_t x, uint8_t y, const char* text, uint8_t char_attr) +{ + if (y >= video_lines || text == nullptr) { + return; + } + while ((x < video_cols) && (*text != 0)) { + buffer[y][x] = *text; + ++text; + ++x; + } +} + +void AP_OSD_SITL::clear(void) +{ + memset(buffer, 0, sizeof(buffer)); +} + +void AP_OSD_SITL::flush(void) +{ + if (!w->isOpen()) { + return; + } + w->clear(); + for (uint8_t y=0; ydraw(s); + } + } + w->display(); +} + +bool AP_OSD_SITL::init(void) +{ + load_font(); + w = new sf::RenderWindow(sf::VideoMode(video_cols*(char_width+char_spacing)*char_scale, + video_lines*(char_height+char_spacing)*char_scale), + "OSD"); + if (!w) { + return false; + } + return true; +} + +AP_OSD_Backend *AP_OSD_SITL::probe(AP_OSD &osd) +{ + AP_OSD_SITL *backend = new AP_OSD_SITL(osd); + if (!backend) { + return nullptr; + } + if (!backend->init()) { + delete backend; + return nullptr; + } + return backend; +} + +AP_OSD_SITL::AP_OSD_SITL(AP_OSD &osd): + AP_OSD_Backend(osd) +{ +} + +#endif // WITH_SITL_OSD diff --git a/libraries/AP_OSD/AP_OSD_SITL.h b/libraries/AP_OSD/AP_OSD_SITL.h new file mode 100644 index 0000000000..43abcd93ff --- /dev/null +++ b/libraries/AP_OSD/AP_OSD_SITL.h @@ -0,0 +1,63 @@ +/* + * This file 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 file 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 . + * + */ +#pragma once + +#ifdef WITH_SITL_OSD + +#include +#include + +class AP_OSD_SITL : public AP_OSD_Backend { + +public: + static AP_OSD_Backend *probe(AP_OSD &osd); + + //draw given text to framebuffer + void write(uint8_t x, uint8_t y, const char* text, uint8_t char_attr) override; + + //initilize display port and underlying hardware + bool init() override; + + //flush framebuffer to screen + void flush() override; + + //clear framebuffer + void clear() override; + +private: + //constructor + AP_OSD_SITL(AP_OSD &osd); + + sf::RenderWindow *w; + + sf::Texture font[256]; + + // setup to match MAX7456 layout + static const uint8_t char_width = 12; + static const uint8_t char_height = 18; + static const uint8_t video_lines = 16; // PAL + static const uint8_t video_cols = 30; + static const uint8_t char_spacing = 1; + + // scaling factor to make it easier to read + static const uint8_t char_scale = 2; + + uint8_t buffer[video_lines][video_cols]; + + void load_font(); +}; + +#endif // WITH_SITL_OSD