AP_Notify: add SITL RGBLed

libraries/AP_Notify/AP_Notify.cpp

@@ -31,6 +31,7 @@
 #include "DiscoLED.h"
 #include "Led_Sysfs.h"
 #include "UAVCAN_RGB_LED.h"
+#include "SITL_SFML_LED.h"
 #include <stdio.h>
 #include "AP_BoardLED2.h"
 
@@ -298,6 +299,9 @@ void AP_Notify::add_backends(void)
 
 #elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
     ADD_BACKEND(new AP_ToneAlarm());
+#ifdef WITH_SITL_RGBLED
+    ADD_BACKEND(new SITL_SFML_LED());
+#endif
 #endif // Noise makers
 
 }

libraries/AP_Notify/SITL_SFML_LED.cpp

@@ -0,0 +1,83 @@
+/*
+   Copyright (C) 2019 Peter Barker. All rights reserved.
+
+   This program 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 program 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/>.
+ */
+#include <AP_HAL/AP_HAL.h>
+
+#include <stdio.h>
+#include <unistd.h>
+
+#ifdef WITH_SITL_RGBLED
+
+#include "SITL_SFML_LED.h"
+
+SITL_SFML_LED::SITL_SFML_LED():
+    RGBLed((uint8_t)brightness::LED_OFF,
+           (uint8_t)brightness::LED_HIGH,
+           (uint8_t)brightness::LED_MEDIUM,
+           (uint8_t)brightness::LED_LOW)
+{
+}
+
+void SITL_SFML_LED::update_thread(void)
+{
+    sf::RenderWindow *w = new sf::RenderWindow(sf::VideoMode(width, height), "LED");
+    if (!w) {
+        AP_HAL::panic("Unable to create RGBLed window");
+    }
+
+    while (w->isOpen()) {
+        sf::Event event;
+        while (w->pollEvent(event)) {
+            if (event.type == sf::Event::Closed) {
+                w->close();
+            }
+        }
+        uint32_t colour = red<<16 | green<<8 | blue;
+        if (colour == last_colour) {
+            usleep(10000);
+            continue;
+        }
+        last_colour = colour;
+
+        w->clear(sf::Color(red, green, blue, 255));
+
+        w->display();
+    }
+}
+
+// trampoline for update thread
+void *SITL_SFML_LED::update_thread_start(void *obj)
+{
+    ((SITL_SFML_LED *)obj)->update_thread();
+    return nullptr;
+}
+
+bool SITL_SFML_LED::hw_init()
+{
+    pthread_create(&thread, NULL, update_thread_start, this);
+
+    return true;
+}
+
+bool SITL_SFML_LED::hw_set_rgb(uint8_t _red, uint8_t _green, uint8_t _blue)
+{
+    red = _red;
+    green = _green;
+    blue = _blue;
+
+    return true;
+}
+#endif

libraries/AP_Notify/SITL_SFML_LED.h

@@ -0,0 +1,63 @@
+/*
+  Copyright (C) 2019 Peter Barker. All rights reserved.
+
+  This program 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 program 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
+
+#include <AP_HAL/AP_HAL.h>
+
+#ifdef WITH_SITL_RGBLED
+
+#include "RGBLed.h"
+
+#ifdef HAVE_SFML_GRAPHICS_H
+#include <SFML/Graphics.h>
+#else
+#include <SFML/Graphics.hpp>
+#endif
+
+class SITL_SFML_LED: public RGBLed
+{
+public:
+    SITL_SFML_LED();
+
+protected:
+    bool hw_init(void) override;
+    bool hw_set_rgb(uint8_t r, uint8_t g, uint8_t b) override;
+
+private:
+
+    pthread_t thread;
+    void update_thread(void);
+    static void *update_thread_start(void *obj);
+
+    static constexpr uint8_t height = 50;
+    static constexpr uint8_t width = height;
+
+    enum class brightness {
+        LED_LOW    = 0x33,
+        LED_MEDIUM = 0x7F,
+        LED_HIGH   = 0xFF,
+        LED_OFF    = 0x00
+    };
+
+    uint32_t last_colour;
+
+    uint8_t red;
+    uint8_t green;
+    uint8_t blue;
+};
+
+#endif