From 7d4a6795d8545572861a7e4fbec23e4da065fc89 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 4 Jul 2018 19:30:30 +1000
Subject: [PATCH] AP_OSD: use a thread for SITL OSD

this allows for window scaling and should fix ubuntu 18 warning
---
 libraries/AP_OSD/AP_OSD_SITL.cpp | 74 ++++++++++++++++++++++++--------
 libraries/AP_OSD/AP_OSD_SITL.h   |  7 +++
 2 files changed, 63 insertions(+), 18 deletions(-)

diff --git a/libraries/AP_OSD/AP_OSD_SITL.cpp b/libraries/AP_OSD/AP_OSD_SITL.cpp
index 7dcb876db9..5212be614d 100644
--- a/libraries/AP_OSD/AP_OSD_SITL.cpp
+++ b/libraries/AP_OSD/AP_OSD_SITL.cpp
@@ -32,7 +32,9 @@
 #include <sys/stat.h>
 #include <fcntl.h>
 #include <unistd.h>
+#include "pthread.h"
 
+extern const AP_HAL::HAL &hal;
 
 /*
   load *.bin font file, in MAX7456 format
@@ -92,47 +94,83 @@ void AP_OSD_SITL::write(uint8_t x, uint8_t y, const char* text, uint8_t char_att
     if (y >= video_lines || text == nullptr) {
         return;
     }
+    mutex->take_blocking();
     while ((x < video_cols) && (*text != 0)) {
         buffer[y][x] = *text;
         ++text;
         ++x;
     }
+    mutex->give();
 }
 
 void AP_OSD_SITL::clear(void)
 {
+    mutex->take_blocking();
     memset(buffer, 0, sizeof(buffer));
+    mutex->give();
 }
 
 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();
+    counter++;
 }
 
-bool AP_OSD_SITL::init(void)
+// main loop of graphics thread
+void AP_OSD_SITL::update_thread(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;
+        AP_HAL::panic("Unable to create OSD window");
     }
+    while (w->isOpen())
+    {
+        sf::Event event;
+        while (w->pollEvent(event))
+        {
+            if (event.type == sf::Event::Closed)
+                w->close();
+        }
+        if (counter == last_counter) {
+            continue;
+        }
+        last_counter = counter;
+
+        uint8_t buffer2[video_lines][video_cols];
+        mutex->take_blocking();
+        memcpy(buffer2, buffer, sizeof(buffer2));
+        mutex->give();
+        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[buffer2[y][x]]);
+                s.setPosition(sf::Vector2f(px, py));
+                s.scale(sf::Vector2f(char_scale,char_scale));
+                w->draw(s);
+            }
+        }
+        w->display();
+        usleep(10000);
+    }
+}
+
+// trampoline for update thread
+void *AP_OSD_SITL::update_thread_start(void *obj)
+{
+    ((AP_OSD_SITL *)obj)->update_thread();
+    return nullptr;
+}
+
+// initialise backend
+bool AP_OSD_SITL::init(void)
+{
+    mutex = hal.util->new_semaphore();
+    pthread_create(&thread, NULL, update_thread_start, this);
     return true;
 }
 
diff --git a/libraries/AP_OSD/AP_OSD_SITL.h b/libraries/AP_OSD/AP_OSD_SITL.h
index f8964e610f..9cffadc297 100644
--- a/libraries/AP_OSD/AP_OSD_SITL.h
+++ b/libraries/AP_OSD/AP_OSD_SITL.h
@@ -62,7 +62,14 @@ private:
     
     uint8_t buffer[video_lines][video_cols];
 
+    void update_thread();
+    static void *update_thread_start(void *obj);
     void load_font();
+
+    pthread_t thread;
+    AP_HAL::Semaphore *mutex;
+    uint32_t counter;
+    uint32_t last_counter;
 };
 
 #endif // WITH_SITL_OSD