ardupilot/libraries/SITL/SIM_LED_n.cpp

Ignoring revisions in .git-blame-ignore-revs. Click here to bypass and see the normal blame view.

98 lines
2.8 KiB
C++
Raw Normal View History

#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);
}
2024-07-11 09:25:32 -03:00
template class SIM_LED_n<1>;
template class SIM_LED_n<2>;
template class SIM_LED_n<3>;
#endif // AP_SIM_LED_N_ENABLED