AP_Notify: add support for ProfiLED over SPI

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)
 {

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);
 

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;
 };

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 <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
@@ -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<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;
+    }
+}

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<AP_HAL::SPIDevice> _dev;
+    void _timer();
+    // perdiodic tick to re-init
+    uint32_t    _last_init_ms;
+    void update_led_strip();
+};