diff --git a/libraries/AP_Notify/SITL_SFML_LED.cpp b/libraries/AP_Notify/SITL_SFML_LED.cpp index aa00721642..ff03d7b852 100644 --- a/libraries/AP_Notify/SITL_SFML_LED.cpp +++ b/libraries/AP_Notify/SITL_SFML_LED.cpp @@ -33,13 +33,90 @@ SITL_SFML_LED::SITL_SFML_LED(): { } +/* + return layout size for a LED layout scheme + */ +bool SITL_SFML_LED::layout_size(SITL::LedLayout layout, uint16_t &xsize, uint16_t &ysize) +{ + switch (layout) { + case SITL::LedLayout::ROWS: + xsize = MAX_LEDS; + ysize = 16; + break; + + case SITL::LedLayout::LUMINOUSBEE: + xsize = 5 + 3 + 5; + ysize = 5 + 3 + 5; + break; + default: + return false; + } + return true; +} + +/* + return layout position for a LED layout scheme + */ +bool SITL_SFML_LED::layout_pos(SITL::LedLayout layout, uint8_t chan, uint8_t led, uint16_t &xpos, uint16_t &ypos) +{ + switch (layout) { + case SITL::LedLayout::ROWS: + xpos = led; + ypos = chan; + break; + + case SITL::LedLayout::LUMINOUSBEE: + /* + Luminousbee has 60 LEDs, 15 on each arm, connected on PWM7 + */ + if (chan != 6) { + return false; + } + if (led < 15) { + xpos = 5 + led / 5; + ypos = led % 5; + if (((led/5) & 1) == 0) { + ypos = 4 - ypos; + } + } else if (led < 30) { + led -= 15; + xpos = led % 5; + ypos = 5 + led / 5; + if (((led/5) & 1) == 1) { + xpos = 4 - xpos; + } + xpos += 8; + } else if (led < 45) { + led -= 30; + xpos = 5 + led / 5; + ypos = led % 5; + if (((led/5) & 1) == 0) { + ypos = 4 - ypos; + } + ypos += 8; + } else if (led < 60) { + led -= 45; + xpos = led % 5; + ypos = 5 + led / 5; + if (((led/5) & 1) == 0) { + xpos = 4 - xpos; + } + } else { + return false; + } + break; + default: + return false; + } + return true; +} + /* update simulation of WS2812 (NeoPixel) serial LEDs */ void SITL_SFML_LED::update_serial_LEDs() { static sf::RenderWindow *w; - const uint8_t MAX_LEDS = 64; static sf::RectangleShape *leds[16][MAX_LEDS]; SITL::SITL *sitl = AP::sitl(); @@ -47,12 +124,17 @@ void SITL_SFML_LED::update_serial_LEDs() // no SerialLEDs set return; } + SITL::LedLayout layout = SITL::LedLayout(sitl->led_layout.get()); if (w == nullptr) { uint8_t max_leds = 0; for (uint8_t i=0; i<16; i++) { max_leds = MAX(max_leds, sitl->led.num_leds[i]); } - w = new sf::RenderWindow(sf::VideoMode(MAX_LEDS*(serialLED_size+1), 16*(serialLED_size+1)), "SerialLED"); + uint16_t xsize=0, ysize=0; + if (!layout_size(layout, xsize, ysize)) { + return; + } + w = new sf::RenderWindow(sf::VideoMode(xsize*(serialLED_size+1), ysize*(serialLED_size+1)), "SerialLED"); if (!w) { return; } @@ -68,7 +150,10 @@ void SITL_SFML_LED::update_serial_LEDs() if (!leds[chan][led]) { return; } - leds[chan][led]->setPosition(led*(serialLED_size+1), chan*(serialLED_size+1)); + uint16_t xpos, ypos; + if (layout_pos(layout, chan, led, xpos, ypos)) { + leds[chan][led]->setPosition(xpos*(serialLED_size+1), ypos*(serialLED_size+1)); + } } leds[chan][led]->setFillColor(sf::Color(rgb[0], rgb[1], rgb[2], 255)); w->draw(*leds[chan][led]); diff --git a/libraries/AP_Notify/SITL_SFML_LED.h b/libraries/AP_Notify/SITL_SFML_LED.h index 98655de392..5a5ebe9fef 100644 --- a/libraries/AP_Notify/SITL_SFML_LED.h +++ b/libraries/AP_Notify/SITL_SFML_LED.h @@ -17,6 +17,7 @@ #pragma once #include +#include #ifdef WITH_SITL_RGBLED @@ -61,6 +62,12 @@ private: uint8_t red; uint8_t green; uint8_t blue; + + static const uint8_t MAX_LEDS = 64; + + // LED layout control + bool layout_size(SITL::LedLayout layout, uint16_t &xsize, uint16_t &ysize); + bool layout_pos(SITL::LedLayout layout, uint8_t chan, uint8_t led, uint16_t &xpos, uint16_t &ypos); }; #endif