From a3a250c3d6539798cf5248e9a8f2c300077bf818 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Tue, 9 Jul 2024 20:12:28 +1000
Subject: [PATCH] SITL: add support for simulated GPIO LEDs

---
 libraries/SITL/SIM_Aircraft.cpp     | 10 +++
 libraries/SITL/SIM_Aircraft.h       | 15 +++++
 libraries/SITL/SIM_GPIO_LED_2.cpp   | 32 ++++++++++
 libraries/SITL/SIM_GPIO_LED_2.h     | 49 +++++++++++++++
 libraries/SITL/SIM_GPIO_LED_3.cpp   | 33 ++++++++++
 libraries/SITL/SIM_GPIO_LED_3.h     | 52 ++++++++++++++++
 libraries/SITL/SIM_GPIO_LED_RGB.cpp | 38 ++++++++++++
 libraries/SITL/SIM_GPIO_LED_RGB.h   | 54 ++++++++++++++++
 libraries/SITL/SIM_LED_n.cpp        | 96 +++++++++++++++++++++++++++++
 libraries/SITL/SIM_LED_n.h          | 62 +++++++++++++++++++
 libraries/SITL/SIM_config.h         | 16 +++++
 11 files changed, 457 insertions(+)
 create mode 100644 libraries/SITL/SIM_GPIO_LED_2.cpp
 create mode 100644 libraries/SITL/SIM_GPIO_LED_2.h
 create mode 100644 libraries/SITL/SIM_GPIO_LED_3.cpp
 create mode 100644 libraries/SITL/SIM_GPIO_LED_3.h
 create mode 100644 libraries/SITL/SIM_GPIO_LED_RGB.cpp
 create mode 100644 libraries/SITL/SIM_GPIO_LED_RGB.h
 create mode 100644 libraries/SITL/SIM_LED_n.cpp
 create mode 100644 libraries/SITL/SIM_LED_n.h

diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp
index 4cc7284526e39d..366ec909304ace 100644
--- a/libraries/SITL/SIM_Aircraft.cpp
+++ b/libraries/SITL/SIM_Aircraft.cpp
@@ -1093,6 +1093,16 @@ void Aircraft::update_external_payload(const struct sitl_input &input)
         dronecan->update();
     }
 #endif
+
+#if AP_SIM_GPIO_LED_2_ENABLED
+    sim_led2.update(*this);
+#endif
+#if AP_SIM_GPIO_LED_3_ENABLED
+    sim_led3.update(*this);
+#endif
+#if AP_SIM_GPIO_LED_RGB_ENABLED
+    sim_ledrgb.update(*this);
+#endif
 }
 
 void Aircraft::add_shove_forces(Vector3f &rot_accel, Vector3f &body_accel)
diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h
index 56ed55860d1f7a..96625afbe083fd 100644
--- a/libraries/SITL/SIM_Aircraft.h
+++ b/libraries/SITL/SIM_Aircraft.h
@@ -38,6 +38,9 @@
 #include <Filter/Filter.h>
 #include "SIM_JSON_Master.h"
 #include "ServoModel.h"
+#include "SIM_GPIO_LED_2.h"
+#include "SIM_GPIO_LED_3.h"
+#include "SIM_GPIO_LED_RGB.h"
 
 namespace SITL {
 
@@ -372,6 +375,18 @@ class Aircraft {
 #if AP_TEST_DRONECAN_DRIVERS
     DroneCANDevice *dronecan;
 #endif
+
+
+#if AP_SIM_GPIO_LED_2_ENABLED
+    GPIO_LED_2 sim_led2{13, 14};  // pins to match boards.py
+#endif
+#if AP_SIM_GPIO_LED_3_ENABLED
+    GPIO_LED_3 sim_led3{13, 14, 15};  // pins to match boards.py
+#endif
+#if AP_SIM_GPIO_LED_RGB_ENABLED
+    GPIO_LED_RGB sim_ledrgb{13, 14, 15};  // pins to match boards.py
+#endif
+
 };
 
 } // namespace SITL
diff --git a/libraries/SITL/SIM_GPIO_LED_2.cpp b/libraries/SITL/SIM_GPIO_LED_2.cpp
new file mode 100644
index 00000000000000..429e3d88753a51
--- /dev/null
+++ b/libraries/SITL/SIM_GPIO_LED_2.cpp
@@ -0,0 +1,32 @@
+#include "SIM_config.h"
+
+#if AP_SIM_GPIO_LED_2_ENABLED
+
+#include "SIM_GPIO_LED_2.h"
+
+#include <SITL/SITL.h>
+
+using namespace SITL;
+
+void GPIO_LED_2::init()
+{
+    leds.init();
+}
+
+void GPIO_LED_2::update(const class Aircraft &aircraft)
+{
+    if (!init_done) {
+        init();
+        init_done = true;
+    }
+
+    const uint16_t pin_mask = AP::sitl()->pin_mask.get();
+    const bool new_led_states[2] {
+        ((pin_mask & uint16_t((1U<<LED_A_PIN))) != 0),
+        ((pin_mask & uint16_t((1U<<LED_B_PIN))) != 0)
+    };
+
+    leds.set_state(new_led_states);
+}
+
+#endif
diff --git a/libraries/SITL/SIM_GPIO_LED_2.h b/libraries/SITL/SIM_GPIO_LED_2.h
new file mode 100644
index 00000000000000..58b93818f8d6d6
--- /dev/null
+++ b/libraries/SITL/SIM_GPIO_LED_2.h
@@ -0,0 +1,49 @@
+/*
+
+  Simulator for GPIO-based "Board LEDs"
+
+  ./Tools/autotest/sim_vehicle.py -v ArduCopter --gdb --debug --rgbled
+
+  reboot
+
+ */
+
+
+#include "SIM_config.h"
+
+#if AP_SIM_GPIO_LED_2_ENABLED
+
+#include "SIM_LED_n.h"
+
+namespace SITL {
+
+class GPIO_LED_2
+{
+public:
+
+    GPIO_LED_2(uint8_t _LED_A_PIN, uint8_t _LED_B_PIN) :
+        LED_A_PIN{_LED_A_PIN},
+        LED_B_PIN{_LED_B_PIN}
+        { }
+
+    void update(const class Aircraft &aircraft);
+
+private:
+
+    void init();
+    bool init_done;
+
+    SIM_LED_n<2>::LEDColour colours[2] {
+        SIM_LED_n<2>::LEDColour::RED,
+        SIM_LED_n<2>::LEDColour::BLUE,
+    };
+
+    SIM_LED_n<2> leds{"GPIO_LED_2", colours};
+
+    uint8_t LED_A_PIN;
+    uint8_t LED_B_PIN;
+};
+
+} // namespace SITL
+
+#endif  // AP_SIM_GPIO_LED_2_ENABLED
diff --git a/libraries/SITL/SIM_GPIO_LED_3.cpp b/libraries/SITL/SIM_GPIO_LED_3.cpp
new file mode 100644
index 00000000000000..d078f30d30bf6a
--- /dev/null
+++ b/libraries/SITL/SIM_GPIO_LED_3.cpp
@@ -0,0 +1,33 @@
+#include "SIM_config.h"
+
+#if AP_SIM_GPIO_LED_3_ENABLED
+
+#include "SIM_GPIO_LED_3.h"
+
+#include <SITL/SITL.h>
+
+using namespace SITL;
+
+void GPIO_LED_3::init()
+{
+    leds.init();
+}
+
+void GPIO_LED_3::update(const class Aircraft &aircraft)
+{
+    if (!init_done) {
+        init();
+        init_done = true;
+    }
+
+    const uint16_t pin_mask = AP::sitl()->pin_mask.get();
+    const bool new_led_states[3] {
+        ((pin_mask & uint16_t((1U<<LED_A_PIN))) != 0),
+        ((pin_mask & uint16_t((1U<<LED_B_PIN))) != 0),
+        ((pin_mask & uint16_t((1U<<LED_C_PIN))) != 0)
+    };
+
+    leds.set_state(new_led_states);
+}
+
+#endif
diff --git a/libraries/SITL/SIM_GPIO_LED_3.h b/libraries/SITL/SIM_GPIO_LED_3.h
new file mode 100644
index 00000000000000..c8849500d32ea2
--- /dev/null
+++ b/libraries/SITL/SIM_GPIO_LED_3.h
@@ -0,0 +1,52 @@
+/*
+
+  Simulator for GPIO-based "Board LEDs"
+
+  ./Tools/autotest/sim_vehicle.py -v ArduCopter --gdb --debug --rgbled
+
+  reboot
+
+ */
+
+
+#include "SIM_config.h"
+
+#if AP_SIM_GPIO_LED_3_ENABLED
+
+#include "SIM_LED_n.h"
+
+namespace SITL {
+
+class GPIO_LED_3
+{
+public:
+
+    GPIO_LED_3(uint8_t _LED_A_PIN, uint8_t _LED_B_PIN, uint8_t _LED_C_PIN) :
+        LED_A_PIN{_LED_A_PIN},
+        LED_B_PIN{_LED_B_PIN},
+        LED_C_PIN{_LED_C_PIN}
+        { }
+
+    void update(const class Aircraft &aircraft);
+
+private:
+
+    void init();
+    bool init_done;
+
+    SIM_LED_n<3>::LEDColour colours[3] {
+        SIM_LED_n<3>::LEDColour::RED,
+        SIM_LED_n<3>::LEDColour::BLUE,
+        SIM_LED_n<3>::LEDColour::YELLOW,
+    };
+
+    SIM_LED_n<3> leds{"GPIO_LED_3", colours};
+
+    uint8_t LED_A_PIN;
+    uint8_t LED_B_PIN;
+    uint8_t LED_C_PIN;
+};
+
+} // namespace SITL
+
+#endif  // AP_SIM_GPIO_LED_3_ENABLED
diff --git a/libraries/SITL/SIM_GPIO_LED_RGB.cpp b/libraries/SITL/SIM_GPIO_LED_RGB.cpp
new file mode 100644
index 00000000000000..940240c6c4b67e
--- /dev/null
+++ b/libraries/SITL/SIM_GPIO_LED_RGB.cpp
@@ -0,0 +1,38 @@
+#include "SIM_config.h"
+
+#if AP_SIM_GPIO_LED_RGB_ENABLED
+
+#include "SIM_GPIO_LED_RGB.h"
+
+#include <SITL/SITL.h>
+
+using namespace SITL;
+
+void GPIO_LED_RGB::init()
+{
+    leds.init();
+    rgbled.init();
+}
+
+void GPIO_LED_RGB::update(const class Aircraft &aircraft)
+{
+    if (!init_done) {
+        init();
+        init_done = true;
+    }
+
+    const uint16_t pin_mask = AP::sitl()->pin_mask.get();
+
+    const bool red = ((pin_mask & uint16_t((1U<<LED_RED_PIN))) != 0);
+    const bool green = ((pin_mask & uint16_t((1U<<LED_GREEN_PIN))) != 0);
+    const bool blue = ((pin_mask & uint16_t((1U<<LED_BLUE_PIN))) != 0);
+
+    const bool new_led_states[3] { red, green, blue };
+    leds.set_state(new_led_states);
+
+    // FIXME:  check why  we need  to  "!" here;  do we  need to  move
+    // ON_VALUE into here?
+    rgbled.set_colours((!red)*255, (!green)*255, (!blue)*255);
+}
+
+#endif
diff --git a/libraries/SITL/SIM_GPIO_LED_RGB.h b/libraries/SITL/SIM_GPIO_LED_RGB.h
new file mode 100644
index 00000000000000..aed4f60d148ee6
--- /dev/null
+++ b/libraries/SITL/SIM_GPIO_LED_RGB.h
@@ -0,0 +1,54 @@
+/*
+
+  Simulator for GPIO-based "Board LEDs"
+
+  ./Tools/autotest/sim_vehicle.py -v ArduCopter --gdb --debug --rgbled
+
+  reboot
+
+ */
+
+
+#include "SIM_config.h"
+
+#if AP_SIM_GPIO_LED_RGB_ENABLED
+
+#include "SIM_LED_n.h"
+#include "SIM_RGBLED.h"
+
+namespace SITL {
+
+class GPIO_LED_RGB
+{
+public:
+
+    GPIO_LED_RGB(uint8_t _LED_RED_PIN, uint8_t _LED_GREEN_PIN, uint8_t _LED_BLUE_PIN) :
+        LED_RED_PIN{_LED_RED_PIN},
+        LED_GREEN_PIN{_LED_GREEN_PIN},
+        LED_BLUE_PIN{_LED_BLUE_PIN}
+        { }
+
+    void update(const class Aircraft &aircraft);
+
+private:
+
+    void init();
+    bool init_done;
+
+    SIM_LED_n<3>::LEDColour colours[3] {
+        SIM_LED_n<3>::LEDColour::RED,
+        SIM_LED_n<3>::LEDColour::GREEN,
+        SIM_LED_n<3>::LEDColour::BLUE,
+    };
+
+    SIM_LED_n<3> leds{"GPIO_LED_RGB", colours};
+    SIM_RGBLED rgbled{"GPIO_LED_RGB2"};
+
+    uint8_t LED_RED_PIN;
+    uint8_t LED_GREEN_PIN;
+    uint8_t LED_BLUE_PIN;
+};
+
+} // namespace SITL
+
+#endif  // AP_SIM_GPIO_LED_RGB_ENABLED
diff --git a/libraries/SITL/SIM_LED_n.cpp b/libraries/SITL/SIM_LED_n.cpp
new file mode 100644
index 00000000000000..30cd3b90efde9c
--- /dev/null
+++ b/libraries/SITL/SIM_LED_n.cpp
@@ -0,0 +1,96 @@
+#include "SIM_config.h"
+
+#if AP_SIM_LED_N_ENABLED
+
+#include "SIM_LED_n.h"
+
+#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>
+
+template <uint8_t NUM_LEDS>
+void SIM_LED_n<NUM_LEDS>::update_thread(void)
+{
+    sf::RenderWindow *w = nullptr;
+    {
+        WITH_SEMAPHORE(AP::notify().sf_window_mutex);
+        w = NEW_NOTHROW sf::RenderWindow(sf::VideoMode(total_width, height), name);
+        w->clear(sf::Color(0, 0, 0, 255));  // blacken
+    }
+
+    if (w == nullptr) {
+        AP_HAL::panic("Unable to create SIM_LED_n 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;
+            }
+            if (memcmp(new_state, last_state, sizeof(new_state)) != 0) {
+                memcpy(last_state, new_state, sizeof(last_state));
+                w->clear(sf::Color(0, 0, 0, 255));  // blacken
+                sf::RectangleShape rectangle(sf::Vector2f(width, height));
+                for (uint8_t i=0; i<NUM_LEDS; i++) {
+                    rectangle.setPosition(i*width, 0);
+                    sf::Color led_colour;
+                    if (new_state[i] == ON_VALUE) {
+                        switch (led_colours[i]) {
+                        case LEDColour::RED:
+                            led_colour = sf::Color::Red;
+                            break;
+                        case LEDColour::GREEN:
+                            led_colour = sf::Color::Green;
+                            break;
+                        case LEDColour::BLUE:
+                            led_colour = sf::Color::Blue;
+                            break;
+                        case LEDColour::YELLOW:
+                            led_colour = sf::Color::Yellow;
+                            break;
+                        }
+                    } else {
+                        led_colour = sf::Color::Black;
+                    }
+                    rectangle.setFillColor(led_colour);
+                    w->draw(rectangle);
+                }
+                w->display();
+            }
+        }
+        usleep(10000);
+    }
+}
+
+// trampoline for update thread
+template <uint8_t NUM_LEDS>
+void *SIM_LED_n<NUM_LEDS>::update_thread_start(void *obj)
+{
+    ((SIM_LED_n *)obj)->update_thread();
+    return nullptr;
+}
+
+template <uint8_t NUM_LEDS>
+void SIM_LED_n<NUM_LEDS>::init()
+{
+    pthread_create(&thread, NULL, update_thread_start, this);
+}
+
+template class SIM_LED_n<2>;
+template class SIM_LED_n<3>;
+
+#endif  // AP_SIM_LED_N_ENABLED
diff --git a/libraries/SITL/SIM_LED_n.h b/libraries/SITL/SIM_LED_n.h
new file mode 100644
index 00000000000000..c6f5da09b36132
--- /dev/null
+++ b/libraries/SITL/SIM_LED_n.h
@@ -0,0 +1,62 @@
+#pragma once
+
+#include "SIM_config.h"
+
+#if AP_SIM_LED_N_ENABLED
+
+#include <stdint.h>
+#include <pthread.h>
+
+/*
+  A class to create output of some description or another for a group
+  of N LEDs.
+
+  Hopefully something visual, but perhaps just text
+*/
+
+template <uint8_t NUM_LEDS>
+class SIM_LED_n
+{
+public:
+    enum class LEDColour : uint8_t {
+        RED,
+        GREEN,
+        BLUE,
+        YELLOW,
+    };
+
+    SIM_LED_n<NUM_LEDS>(const char *_name, const LEDColour _led_colours[NUM_LEDS]) :
+        name{_name}
+        {
+            memcpy(led_colours, _led_colours, sizeof(led_colours));
+        }
+
+    void set_state(const bool state[NUM_LEDS]) {
+        memcpy(new_state, state, sizeof(new_state));
+    }
+
+    void init();
+
+private:
+
+    const char *name;
+    LEDColour led_colours[NUM_LEDS];
+
+    static constexpr uint8_t height = 50;
+    static constexpr uint8_t width = height;
+    static constexpr uint8_t total_width = width * NUM_LEDS;
+
+    pthread_t thread;
+    static void *update_thread_start(void *obj);
+    void update_thread(void);
+
+    // state to be written to LEDs; note lack of thread protection.
+    bool new_state[NUM_LEDS];
+
+    // avoid too-frequent display updates:
+    bool last_state[NUM_LEDS];
+
+    const bool ON_VALUE = 0;  // so if the pin is low we are lit
+};
+
+#endif  // AP_SIM_LED_N_ENABLED
diff --git a/libraries/SITL/SIM_config.h b/libraries/SITL/SIM_config.h
index d5f6a00192e6c0..99a53bfb0fd437 100644
--- a/libraries/SITL/SIM_config.h
+++ b/libraries/SITL/SIM_config.h
@@ -23,6 +23,22 @@
 #define AP_SIM_IS31FL3195_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
 #endif
 
+#ifndef AP_SIM_LED_N_ENABLED
+#define AP_SIM_LED_N_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) && defined(WITH_SITL_RGBLED)
+#endif
+
+#ifndef AP_SIM_GPIO_LED_2_ENABLED
+#define AP_SIM_GPIO_LED_2_ENABLED AP_SIM_LED_N_ENABLED && 0
+#endif
+
+#ifndef AP_SIM_GPIO_LED_3_ENABLED
+#define AP_SIM_GPIO_LED_3_ENABLED AP_SIM_LED_N_ENABLED && 0
+#endif
+
+#ifndef AP_SIM_GPIO_LED_RGB_ENABLED
+#define AP_SIM_GPIO_LED_RGB_ENABLED AP_SIM_LED_N_ENABLED
+#endif
+
 #ifndef AP_SIM_LOWEHEISER_ENABLED
 #define AP_SIM_LOWEHEISER_ENABLED AP_SIM_ENABLED && HAL_MAVLINK_BINDINGS_ENABLED
 #endif