ardupilot/libraries/AP_Notify/SITL_SFML_LED.cpp

227 lines
5.9 KiB
C++

/*
Copyright (C) 2019 Peter Barker. All rights reserved.
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <AP_HAL/AP_HAL.h>
#include <AP_Notify/AP_Notify.h>
#include <stdio.h>
#include <unistd.h>
#ifdef WITH_SITL_RGBLED
#include <SITL/SITL.h>
#include "SITL_SFML_LED.h"
SITL_SFML_LED::SITL_SFML_LED():
RGBLed((uint8_t)brightness::LED_OFF,
(uint8_t)brightness::LED_HIGH,
(uint8_t)brightness::LED_MEDIUM,
(uint8_t)brightness::LED_LOW)
{
}
/*
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;
static sf::RectangleShape *leds[16][MAX_LEDS];
SITL::SIM *sitl = AP::sitl();
if (sitl == nullptr || sitl->led.send_counter == 0) {
// 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]);
}
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;
}
w->clear(sf::Color(0, 0, 0, 255));
}
for (uint8_t chan=0; chan<16; chan++) {
for (uint8_t led=0; led<sitl->led.num_leds[chan]; led++) {
uint8_t *rgb = &sitl->led.rgb[chan][led].rgb[0];
if (leds[chan][led] == nullptr) {
leds[chan][led] = new sf::RectangleShape(sf::Vector2f(serialLED_size, serialLED_size));
if (!leds[chan][led]) {
return;
}
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]);
}
}
w->display();
}
void SITL_SFML_LED::update_thread(void)
{
sf::RenderWindow *w = nullptr;
{
WITH_SEMAPHORE(AP::notify().sf_window_mutex);
w = new sf::RenderWindow(sf::VideoMode(width, height), "LED");
}
if (!w) {
AP_HAL::panic("Unable to create 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();
}
update_serial_LEDs();
}
usleep(10000);
}
}
// trampoline for update thread
void *SITL_SFML_LED::update_thread_start(void *obj)
{
((SITL_SFML_LED *)obj)->update_thread();
return nullptr;
}
bool SITL_SFML_LED::init()
{
pthread_create(&thread, NULL, update_thread_start, this);
return true;
}
bool SITL_SFML_LED::hw_set_rgb(uint8_t _red, uint8_t _green, uint8_t _blue)
{
red = _red;
green = _green;
blue = _blue;
return true;
}
#endif