diff --git a/libraries/AP_SerialLED/AP_SerialLED.cpp b/libraries/AP_SerialLED/AP_SerialLED.cpp new file mode 100644 index 0000000000..101855755c --- /dev/null +++ b/libraries/AP_SerialLED/AP_SerialLED.cpp @@ -0,0 +1,48 @@ +/* + 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 . + */ +/* + AP_SerialLED for controlling serial connected LEDs using WS2812B protocol + */ + +#include "AP_SerialLED.h" +#include + +extern const AP_HAL::HAL& hal; + +AP_SerialLED AP_SerialLED::singleton; + +AP_SerialLED::AP_SerialLED() +{ +} + +// set number of LEDs per pin +bool AP_SerialLED::set_num_LEDs(uint8_t chan, uint8_t num_leds) +{ + return hal.rcout->set_neopixel_num_LEDs(chan-1, num_leds); +} + +// set RGB value on mask of LEDs. chan is PWM output, 1..16 +void AP_SerialLED::set_RGB(uint8_t chan, uint32_t ledmask, uint8_t red, uint8_t green, uint8_t blue) +{ + if (chan >= 1 && chan <= 16) { + hal.rcout->set_neopixel_rgb_data(chan-1, ledmask, red, green, blue); + } +} + +// trigger sending of LED changes to LEDs +void AP_SerialLED::send(void) +{ + hal.rcout->neopixel_send(); +} diff --git a/libraries/AP_SerialLED/AP_SerialLED.h b/libraries/AP_SerialLED/AP_SerialLED.h new file mode 100644 index 0000000000..acc845e04a --- /dev/null +++ b/libraries/AP_SerialLED/AP_SerialLED.h @@ -0,0 +1,42 @@ +/* + 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 . + */ +/* + AP_SerialLED for controlling serial connected LEDs using WS2812B protocol + */ +#pragma once + +#include + +class AP_SerialLED { +public: + AP_SerialLED(); + + // set number of LEDs per pin + bool set_num_LEDs(uint8_t chan, uint8_t num_leds); + + // set RGB value on mask of LEDs. chan is PWM output, 1..16 + void set_RGB(uint8_t chan, uint32_t ledmask, uint8_t red, uint8_t green, uint8_t blue); + + // trigger sending of LED changes to LEDs + void send(void); + + // singleton support + static AP_SerialLED *get_singleton(void) { + return &singleton; + } + +private: + static AP_SerialLED singleton; +};