#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<1>; template class SIM_LED_n<2>; template class SIM_LED_n<3>; #endif // AP_SIM_LED_N_ENABLED