From 8f7f0d5e725222fe09d6faf5a91c7bb563d0e287 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Mon, 27 Feb 2023 12:08:53 +1100
Subject: [PATCH] SITL: add and use SIM_RGBLED

---
 libraries/SITL/SIM_RGBLED.cpp     | 65 +++++++++++++++++++++++++++++++
 libraries/SITL/SIM_RGBLED.h       | 43 ++++++++++++++++++++
 libraries/SITL/SIM_ToshibaLED.cpp | 12 ++++++
 libraries/SITL/SIM_ToshibaLED.h   |  6 +++
 4 files changed, 126 insertions(+)
 create mode 100644 libraries/SITL/SIM_RGBLED.cpp
 create mode 100644 libraries/SITL/SIM_RGBLED.h

diff --git a/libraries/SITL/SIM_RGBLED.cpp b/libraries/SITL/SIM_RGBLED.cpp
new file mode 100644
index 0000000000..6e64c47c4b
--- /dev/null
+++ b/libraries/SITL/SIM_RGBLED.cpp
@@ -0,0 +1,65 @@
+#include "SIM_RGBLED.h"
+
+#ifdef WITH_SITL_RGBLED
+
+#include <AP_HAL/HAL.h>
+
+#ifdef HAVE_SFML_GRAPHICS_H
+#include <SFML/Graphics.h>
+#else
+#include <SFML/Graphics.hpp>
+#endif
+
+#include <AP_Notify/AP_Notify.h>
+
+void SIM_RGBLED::update_thread(void)
+{
+    sf::RenderWindow *w = nullptr;
+    {
+        WITH_SEMAPHORE(AP::notify().sf_window_mutex);
+        w = new sf::RenderWindow(sf::VideoMode(width, height), name);
+    }
+
+    if (w == nullptr) {
+        AP_HAL::panic("Unable to create SIM_RGBLED window");
+    }
+
+    while (true) {
+        {
+            WITH_SEMAPHORE(AP::notify().sf_window_mutex);
+            sf::Event event;
+            while (w->pollEvent(event)) {
+                if (event.type == sf::Event::Closed) {
+                    w->close();
+                    break;
+                }
+            }
+            if (!w->isOpen()) {
+                break;
+            }
+            const uint32_t colour = red<<16 | green<<8 | blue;
+            if (colour != last_colour) {
+                last_colour = colour;
+                w->clear(sf::Color(red, green, blue, 255));
+                w->display();
+            }
+        }
+        usleep(10000);
+    }
+}
+
+// trampoline for update thread
+void *SIM_RGBLED::update_thread_start(void *obj)
+{
+    ((SIM_RGBLED *)obj)->update_thread();
+    return nullptr;
+}
+
+#endif  // WITH_SITL_RGBLED
+
+void SIM_RGBLED::init()
+{
+#ifdef WITH_SITL_RGBLED
+    pthread_create(&thread, NULL, update_thread_start, this);
+#endif
+}
diff --git a/libraries/SITL/SIM_RGBLED.h b/libraries/SITL/SIM_RGBLED.h
new file mode 100644
index 0000000000..29296420f5
--- /dev/null
+++ b/libraries/SITL/SIM_RGBLED.h
@@ -0,0 +1,43 @@
+#pragma once
+
+#include <stdint.h>
+#include <pthread.h>
+
+/*
+  A class to create output of some description or another for RGB LEDs.
+
+  Hopefully something visual, but perhaps just text
+*/
+
+class SIM_RGBLED
+{
+public:
+    SIM_RGBLED(const char *_name) :
+        name{_name}
+        { }
+
+    void init();
+
+    void set_colours(uint8_t _red, uint8_t _green, uint8_t _blue) {
+        red = _red;
+        green = _green;
+        blue = _blue;
+    }
+
+private:
+
+    const char *name;
+
+    uint8_t red;
+    uint8_t green;
+    uint8_t blue;
+
+    static constexpr uint8_t height = 50;
+    static constexpr uint8_t width = height;
+
+    pthread_t thread;
+    static void *update_thread_start(void *obj);
+    void update_thread(void);
+
+    uint32_t last_colour;
+};
diff --git a/libraries/SITL/SIM_ToshibaLED.cpp b/libraries/SITL/SIM_ToshibaLED.cpp
index b6b92cf4a2..7d54c7cc7f 100644
--- a/libraries/SITL/SIM_ToshibaLED.cpp
+++ b/libraries/SITL/SIM_ToshibaLED.cpp
@@ -18,6 +18,18 @@ void SITL::ToshibaLED::update(const class Aircraft &aircraft)
     last_print_pwm2 = get_register(ToshibaLEDDevReg::PWM2);
     last_print_enable = get_register(ToshibaLEDDevReg::ENABLE);
     // gcs().send_text(MAV_SEVERITY_INFO, "SIM_ToshibaLED: PWM0=%u PWM1=%u PWM2=%u ENABLE=%u", last_print_pwm0, last_print_pwm1, last_print_pwm2, last_print_enable);
+
+    if (get_register(ToshibaLEDDevReg::ENABLE)) {
+        // here we convert from 0-15 BGR (the PWM values from the i2c bus)
+        // to 0-255 RGB (what SIM_RGBLED wants):
+        rgbled.set_colours(
+            get_register(ToshibaLEDDevReg::PWM2) * 17,
+            get_register(ToshibaLEDDevReg::PWM1) * 17,
+            get_register(ToshibaLEDDevReg::PWM0) * 17
+            );
+    } else {
+        rgbled.set_colours(0, 0, 0);
+    }
 }
 
 #endif
diff --git a/libraries/SITL/SIM_ToshibaLED.h b/libraries/SITL/SIM_ToshibaLED.h
index 12a03ff986..b7c5fd83fd 100644
--- a/libraries/SITL/SIM_ToshibaLED.h
+++ b/libraries/SITL/SIM_ToshibaLED.h
@@ -8,6 +8,8 @@
 
 #if AP_SIM_TOSHIBALED_ENABLED
 
+#include "SIM_RGBLED.h"
+
 namespace SITL {
 
 class ToshibaLEDDevReg : public I2CRegEnum {
@@ -22,6 +24,8 @@ class ToshibaLED : public I2CDevice, protected I2CRegisters_8Bit
 {
 public:
     void init() override {
+        rgbled.init();
+
         add_register("PWM0", ToshibaLEDDevReg::PWM0, I2CRegisters::RegMode::WRONLY);
         add_register("PWM1", ToshibaLEDDevReg::PWM1, I2CRegisters::RegMode::WRONLY);
         add_register("PWM2", ToshibaLEDDevReg::PWM2, I2CRegisters::RegMode::WRONLY);
@@ -39,6 +43,8 @@ private:
     uint8_t last_print_pwm1;
     uint8_t last_print_pwm2;
     uint8_t last_print_enable;
+
+    SIM_RGBLED rgbled{"ToshibaLED"};
 };
 
 } // namespace SITL