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 + +#ifdef HAVE_SFML_GRAPHICS_H +#include +#else +#include +#endif + +#include + +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 +#include + +/* + 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