diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp index d5d458199a..8e6fa15f7b 100644 --- a/libraries/AP_Notify/AP_Notify.cpp +++ b/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 #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 } diff --git a/libraries/AP_Notify/SITL_SFML_LED.cpp b/libraries/AP_Notify/SITL_SFML_LED.cpp new file mode 100644 index 0000000000..fd4ec469f8 --- /dev/null +++ b/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 . + */ +#include + +#include +#include + +#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 diff --git a/libraries/AP_Notify/SITL_SFML_LED.h b/libraries/AP_Notify/SITL_SFML_LED.h new file mode 100644 index 0000000000..ba5366bb92 --- /dev/null +++ b/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 . +*/ +#pragma once + +#include + +#ifdef WITH_SITL_RGBLED + +#include "RGBLed.h" + +#ifdef HAVE_SFML_GRAPHICS_H +#include +#else +#include +#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