AP_OSD: added SITL OSD backend

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 <AP_HAL/AP_HAL.h>
 #include <AP_HAL/Util.h>
 #include <RC_Channel/RC_Channel.h>
@@ -86,7 +89,7 @@ void AP_OSD::init()
     default:
         break;
 
-    case OSD_MAX7456:
+    case OSD_MAX7456: {
         AP_HAL::OwnPtr<AP_HAL::Device> 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()

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,

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 <http://www.gnu.org/licenses/>.
+ *
+ */
+/*
+  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 <AP_HAL/Util.h>
+#include <AP_HAL/Semaphores.h>
+#include <AP_HAL/Scheduler.h>
+#include <AP_ROMFS/AP_ROMFS.h>
+#include <utility>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <unistd.h>
+
+
+/*
+  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<char_height; y++) {
+            for (uint16_t x=0; x<char_width; x++) {
+                // 2 bits per pixel
+                uint16_t bitoffset = (y*char_width+x)*2;
+                uint8_t byteoffset = bitoffset / 8;
+                uint8_t bitshift = 6-(bitoffset % 8);
+                uint8_t v = (c[byteoffset] >> 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; y<video_lines; y++) {
+        for (uint8_t x=0; x<video_cols; x++) {
+            uint16_t px = x * (char_width+char_spacing) * char_scale;
+            uint16_t py = y * (char_height+char_spacing) * char_scale;
+            sf::Sprite s;
+            s.setTexture(font[buffer[y][x]]);
+            s.setPosition(sf::Vector2f(px, py));
+            s.scale(sf::Vector2f(char_scale,char_scale));
+            w->draw(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

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 <http://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#ifdef WITH_SITL_OSD
+
+#include <AP_OSD/AP_OSD_Backend.h>
+#include <SFML/Graphics.hpp>
+
+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