ardupilot/libraries/AP_Notify/ProfiLED.cpp

164 lines
4.8 KiB
C++

/*
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_Notify/AP_Notify.h"
#include "ProfiLED.h"
#include "SRV_Channel/SRV_Channel.h"
#include <utility>
#include <GCS_MAVLink/GCS.h>
// This limit is from the dshot driver rc out groups limit, we need at least one channel for clock
#define AP_NOTIFY_ProfiLED_MAX_INSTANCES 3
#define ProfiLED_LOW 0x33
#define ProfiLED_MEDIUM 0x7F
#define ProfiLED_HIGH 0xFF
#define ProfiLED_OFF 0x00
extern const AP_HAL::HAL& hal;
ProfiLED::ProfiLED() :
SerialLED(ProfiLED_OFF, ProfiLED_HIGH, ProfiLED_MEDIUM, ProfiLED_LOW)
{
}
uint16_t ProfiLED::init_ports()
{
uint16_t mask = 0;
for (uint16_t i=0; i<AP_NOTIFY_ProfiLED_MAX_INSTANCES; i++) {
const SRV_Channel::Aux_servo_function_t fn = (SRV_Channel::Aux_servo_function_t)((uint8_t)SRV_Channel::k_ProfiLED_1 + i);
if (!SRV_Channels::function_assigned(fn)) {
continue;
}
mask |= SRV_Channels::get_output_channel_mask(fn);
}
if (mask == 0) {
return 0;
}
AP_SerialLED *led = AP_SerialLED::get_singleton();
if (led == nullptr) {
return 0;
}
for (uint16_t chan=0; chan<16; chan++) {
if ((1U<<chan) & mask) {
led->set_num_profiled(chan+1, (pNotify->get_led_len()));
}
}
return mask;
}
ProfiLED_SPI::ProfiLED_SPI() :
RGBLed(ProfiLED_OFF, ProfiLED_HIGH, ProfiLED_MEDIUM, ProfiLED_LOW) {}
bool ProfiLED_SPI::init()
{
num_leds = pNotify->get_led_len() + 1; // for some reason we have to send an additional LED data
rgb = new ProfiLED_SPI::RGB[num_leds];
if (!rgb) {
return false;
}
memset(rgb, 0, num_leds * sizeof(ProfiLED_SPI::RGB));
_dev = std::move(hal.spi->get_device("profiled"));
if (!_dev) {
return false;
}
WITH_SEMAPHORE(_dev->get_semaphore());
_dev->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&ProfiLED_SPI::_timer, void));
_need_update = true;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Initialised ProfiLED over SPIs");
return true;
}
void ProfiLED_SPI::rgb_set_id(uint8_t red, uint8_t green, uint8_t blue, uint8_t id)
{
if (id >= num_leds) {
return;
}
rgb[id] = {blue, red, green};
_need_update = true;
}
bool ProfiLED_SPI::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
{
for (uint16_t i = 0; i < num_leds; i++) {
rgb[i] = {blue, red, green};
}
_need_update = true;
return true;
}
void ProfiLED_SPI::_timer(void)
{
_need_update = false;
update_led_strip();
}
void ProfiLED_SPI::update_led_strip() {
const uint32_t min_bits = num_leds*25+50;
const uint8_t num_leading_zeros = 8-min_bits%8 + 50;
const uint32_t output_stream_byte_length = (min_bits+7)/8;
WITH_SEMAPHORE(_dev->get_semaphore());
uint32_t curr_led_idx = 0;
uint8_t send_buf[32];
uint8_t send_buf_len = 0;
union {
ProfiLED_SPI::RGB struct_val;
uint8_t bytes_val[3];
} curr_led_color;
curr_led_color.struct_val = rgb[curr_led_idx];
for (uint32_t i=0; i<output_stream_byte_length; i++) {
uint8_t byte = 0;
for (uint8_t bit = 0; bit < 8; bit++) {
uint32_t out_bit_idx = i*8+bit;
uint8_t bit_val;
if (out_bit_idx < num_leading_zeros) {
bit_val = 0;
} else if ((out_bit_idx-num_leading_zeros) % 25 == 0) {
bit_val = 1;
} else {
uint32_t in_bit_idx = out_bit_idx - num_leading_zeros - (out_bit_idx - num_leading_zeros)/25;
uint32_t in_led_idx = in_bit_idx/24;
if (curr_led_idx != in_led_idx) {
curr_led_idx = in_led_idx;
curr_led_color.struct_val = rgb[curr_led_idx];
}
bit_val = (curr_led_color.bytes_val[(in_bit_idx%24)/8] >> (8-in_bit_idx%8)) & 1;
}
byte |= bit_val << (7-bit);
}
send_buf[send_buf_len++] = byte;
if (send_buf_len >= 32) {
_dev->transfer(send_buf, send_buf_len, NULL, 0);
send_buf_len = 0;
}
}
if (send_buf_len > 0) {
_dev->transfer(send_buf, send_buf_len, NULL, 0);
send_buf_len = 0;
}
}