Line data    Source code

       1             : #include "SIM_RGBLED.h"
       2             : 
       3             : #ifdef WITH_SITL_RGBLED
       4             : 
       5             : #include <AP_HAL/HAL.h>
       6             : 
       7             : #ifdef HAVE_SFML_GRAPHICS_H
       8             : #include <SFML/Graphics.h>
       9             : #else
      10             : #include <SFML/Graphics.hpp>
      11             : #endif
      12             : 
      13             : #include <AP_Notify/AP_Notify.h>
      14             : 
      15             : void SIM_RGBLED::update_thread(void)
      16             : {
      17             :     sf::RenderWindow *w = nullptr;
      18             :     {
      19             :         WITH_SEMAPHORE(AP::notify().sf_window_mutex);
      20             :         w = NEW_NOTHROW sf::RenderWindow(sf::VideoMode(width, height), name);
      21             :     }
      22             : 
      23             :     if (w == nullptr) {
      24             :         AP_HAL::panic("Unable to create SIM_RGBLED window");
      25             :     }
      26             : 
      27             :     while (true) {
      28             :         {
      29             :             WITH_SEMAPHORE(AP::notify().sf_window_mutex);
      30             :             sf::Event event;
      31             :             while (w->pollEvent(event)) {
      32             :                 if (event.type == sf::Event::Closed) {
      33             :                     w->close();
      34             :                     break;
      35             :                 }
      36             :             }
      37             :             if (!w->isOpen()) {
      38             :                 break;
      39             :             }
      40             :             const uint32_t colour = red<<16 | green<<8 | blue;
      41             :             if (colour != last_colour) {
      42             :                 last_colour = colour;
      43             :                 w->clear(sf::Color(red, green, blue, 255));
      44             :                 w->display();
      45             :             }
      46             :         }
      47             :         usleep(10000);
      48             :     }
      49             : }
      50             : 
      51             : // trampoline for update thread
      52             : void *SIM_RGBLED::update_thread_start(void *obj)
      53             : {
      54             :     ((SIM_RGBLED *)obj)->update_thread();
      55             :     return nullptr;
      56             : }
      57             : 
      58             : #endif  // WITH_SITL_RGBLED
      59             : 
      60        8376 : void SIM_RGBLED::init()
      61             : {
      62             : #ifdef WITH_SITL_RGBLED
      63             :     pthread_create(&thread, NULL, update_thread_start, this);
      64             : #endif
      65        8376 : }