From e22ca196405935abc268747a76f24d293e086348 Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Tue, 1 Sep 2020 01:58:35 +0530 Subject: [PATCH] AP_Notify: add support for ProfiLED over SPI --- libraries/AP_Notify/AP_Notify.cpp | 15 ++++- libraries/AP_Notify/AP_Notify.h | 4 ++ libraries/AP_Notify/NotifyDevice.h | 4 ++ libraries/AP_Notify/ProfiLED.cpp | 101 +++++++++++++++++++++++++++++ libraries/AP_Notify/ProfiLED.h | 28 ++++++++ 5 files changed, 151 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp index fc44486b9e..b435723996 100644 --- a/libraries/AP_Notify/AP_Notify.cpp +++ b/libraries/AP_Notify/AP_Notify.cpp @@ -180,7 +180,7 @@ const AP_Param::GroupInfo AP_Notify::var_info[] = { // @Param: LED_TYPES // @DisplayName: LED Driver Types // @Description: Controls what types of LEDs will be enabled - // @Bitmask: 0:Built-in LED, 1:Internal ToshibaLED, 2:External ToshibaLED, 3:External PCA9685, 4:Oreo LED, 5:UAVCAN, 6:NCP5623 External, 7:NCP5623 Internal, 8:NeoPixel, 9:ProfiLED, 10:Scripting, 11:DShot + // @Bitmask: 0:Built-in LED, 1:Internal ToshibaLED, 2:External ToshibaLED, 3:External PCA9685, 4:Oreo LED, 5:UAVCAN, 6:NCP5623 External, 7:NCP5623 Internal, 8:NeoPixel, 9:ProfiLED, 10:Scripting, 11:DShot, 12:ProfiLED_SPI // @User: Advanced AP_GROUPINFO("LED_TYPES", 6, AP_Notify, _led_type, BUILD_DEFAULT_LED_TYPE), @@ -312,6 +312,9 @@ void AP_Notify::add_backends(void) case Notify_LED_ProfiLED: ADD_BACKEND(new ProfiLED()); break; + case Notify_LED_ProfiLED_SPI: + ADD_BACKEND(new ProfiLED_SPI()); + break; case Notify_LED_OreoLED: #if HAL_OREO_LED_ENABLED if (_oreo_theme) { @@ -424,6 +427,16 @@ void AP_Notify::handle_rgb(uint8_t r, uint8_t g, uint8_t b, uint8_t rate_hz) } } +// handle RGB Per led from Scripting +void AP_Notify::handle_rgb_id(uint8_t r, uint8_t g, uint8_t b, uint8_t id) +{ + for (uint8_t i = 0; i < _num_devices; i++) { + if (_devices[i] != nullptr) { + _devices[i]->rgb_set_id(r, g, b, id); + } + } +} + // handle a PLAY_TUNE message void AP_Notify::handle_play_tune(const mavlink_message_t &msg) { diff --git a/libraries/AP_Notify/AP_Notify.h b/libraries/AP_Notify/AP_Notify.h index 6afb2b8f09..893d4dc8a7 100644 --- a/libraries/AP_Notify/AP_Notify.h +++ b/libraries/AP_Notify/AP_Notify.h @@ -73,6 +73,7 @@ public: Notify_LED_ProfiLED = (1 << 9), // ProfiLED Notify_LED_Scripting = (1 << 10),// Colour accessor for scripting Notify_LED_DShot = (1 << 11),// Use dshot commands to set ESC LEDs + Notify_LED_ProfiLED_SPI = (1 << 12), // ProfiLED Notify_LED_MAX }; @@ -157,6 +158,9 @@ public: // handle RGB from Scripting or AP_Periph static void handle_rgb(uint8_t r, uint8_t g, uint8_t b, uint8_t rate_hz = 0); + // handle RGB from Scripting + static void handle_rgb_id(uint8_t r, uint8_t g, uint8_t b, uint8_t id); + // handle a PLAY_TUNE message static void handle_play_tune(const mavlink_message_t &msg); diff --git a/libraries/AP_Notify/NotifyDevice.h b/libraries/AP_Notify/NotifyDevice.h index b2e91e849c..6a8a4b5695 100644 --- a/libraries/AP_Notify/NotifyDevice.h +++ b/libraries/AP_Notify/NotifyDevice.h @@ -27,6 +27,10 @@ public: // give RGB and flash rate, used with scripting virtual void rgb_control(uint8_t r, uint8_t g, uint8_t b, uint8_t rate_hz) {} + // RGB control multiple leds independently + // give RGB value for single led + virtual void rgb_set_id(uint8_t r, uint8_t g, uint8_t b, uint8_t id) {} + // this pointer is used to read the parameters relative to devices const AP_Notify *pNotify; }; diff --git a/libraries/AP_Notify/ProfiLED.cpp b/libraries/AP_Notify/ProfiLED.cpp index afc56377b3..15945a15d6 100644 --- a/libraries/AP_Notify/ProfiLED.cpp +++ b/libraries/AP_Notify/ProfiLED.cpp @@ -16,6 +16,8 @@ #include "AP_Notify/AP_Notify.h" #include "ProfiLED.h" #include "SRV_Channel/SRV_Channel.h" +#include +#include // 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 @@ -60,3 +62,102 @@ uint16_t ProfiLED::init_ports() 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> (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; + } +} diff --git a/libraries/AP_Notify/ProfiLED.h b/libraries/AP_Notify/ProfiLED.h index 5ea6687680..10fc69e425 100644 --- a/libraries/AP_Notify/ProfiLED.h +++ b/libraries/AP_Notify/ProfiLED.h @@ -25,3 +25,31 @@ public: uint16_t init_ports() override; }; + +class ProfiLED_SPI: public RGBLed { +public: + ProfiLED_SPI(); + + void rgb_set_id(uint8_t r, uint8_t g, uint8_t b, uint8_t id) override; + bool init(void) override; + +protected: + + bool hw_set_rgb(uint8_t r, uint8_t g, uint8_t b) override; + +private: + uint16_t num_leds; + struct RGB { + uint8_t b; + uint8_t r; + uint8_t g; + } *rgb; + + bool _need_update; + + AP_HAL::OwnPtr _dev; + void _timer(); + // perdiodic tick to re-init + uint32_t _last_init_ms; + void update_led_strip(); +};