AP_HAL_ChibiOS: add support for ProfiLEDs

libraries/AP_HAL_ChibiOS/RCOutput.cpp

@@ -399,7 +399,7 @@ void RCOutput::push_local(void)
                     pwmEnableChannel(group.pwm_drv, j, width);
                 }
 #ifndef DISABLE_DSHOT
-                else if (is_dshot_protocol(group.current_mode) || group.current_mode == MODE_NEOPIXEL) {
+                else if (is_dshot_protocol(group.current_mode) || group.current_mode == MODE_NEOPIXEL || group.current_mode == MODE_PROFILED) {
                     // set period_us to time for pulse output, to enable very fast rates
                     period_us = dshot_pulse_time_us;
                 }
@@ -410,6 +410,7 @@ void RCOutput::push_local(void)
                 if (group.current_mode == MODE_PWM_ONESHOT ||
                     group.current_mode == MODE_PWM_ONESHOT125 ||
                     group.current_mode == MODE_NEOPIXEL ||
+                    group.current_mode == MODE_PROFILED ||
                     is_dshot_protocol(group.current_mode)) {
                     need_trigger |= (1U<<i);
                 }
@@ -488,7 +489,7 @@ bool RCOutput::mode_requires_dma(enum output_mode mode) const
 #ifdef DISABLE_DSHOT
     return false;
 #else
-    return is_dshot_protocol(mode) || (mode == MODE_NEOPIXEL);
+    return is_dshot_protocol(mode) || (mode == MODE_NEOPIXEL) || (mode == MODE_PROFILED);
 #endif //#ifdef DISABLE_DSHOT
 }
 
@@ -618,24 +619,32 @@ void RCOutput::set_group_mode(pwm_group &group)
         break;
 
     case MODE_NEOPIXEL:
+    case MODE_PROFILED:
     {
+        uint8_t bits_per_pixel = 24;
+        bool active_high = true;
+
+        if (group.current_mode == MODE_PROFILED) {
+            bits_per_pixel = 25;
+            active_high = false;
+        }
+
         const uint32_t rate = protocol_bitrate(group.current_mode);
         const uint32_t bit_period = 20;
 
         // configure timer driver for DMAR at requested rate
         const uint8_t pad_end_bits = 8;
         const uint8_t pad_start_bits = 1;
-        const uint8_t bits_per_pixel = 24;
         const uint8_t channels_per_group = 4;
-        const uint16_t neopixel_bit_length = bits_per_pixel * channels_per_group * group.neopixel_nleds + (pad_start_bits + pad_end_bits) * channels_per_group;
-        const uint16_t neopixel_buffer_length = neopixel_bit_length * sizeof(uint32_t);
-        if (!setup_group_DMA(group, rate, bit_period, true, neopixel_buffer_length, true)) {
+        const uint16_t bit_length = bits_per_pixel * channels_per_group * group.serial_nleds + (pad_start_bits + pad_end_bits) * channels_per_group;
+        const uint16_t buffer_length = bit_length * sizeof(uint32_t);
+        if (!setup_group_DMA(group, rate, bit_period, active_high, buffer_length, true)) {
             group.current_mode = MODE_PWM_NONE;
             break;
         }
 
         // calculate min time between pulses
-        dshot_pulse_time_us = 1000000UL * neopixel_bit_length / rate;
+        dshot_pulse_time_us = 1000000UL * bit_length / rate;
         break;
     }
 
@@ -901,13 +910,14 @@ void RCOutput::timer_tick(void)
             dshot_send(group, true);
         }
     }
-    if (neopixel_pending && chMtxTryLock(&trigger_mutex)) {
-        neopixel_pending = false;
+    if (serial_led_pending && chMtxTryLock(&trigger_mutex)) {
+        serial_led_pending = false;
         for (uint8_t j = 0; j < NUM_GROUPS; j++) {
             pwm_group &group = pwm_group_list[j];
-            if (group.current_mode == MODE_NEOPIXEL) {
-                // if a group does not sucssed,  try again on the next tick
-                neopixel_pending |= !neopixel_send(group);
+            if (group.serial_led_pending && (group.current_mode == MODE_NEOPIXEL || group.current_mode == MODE_PROFILED)) {
+                group.serial_led_pending = !serial_led_send(group);
+                group.prepared_send = group.serial_led_pending;
+                serial_led_pending |=  group.serial_led_pending;
             }
         }
         chMtxUnlock(&trigger_mutex);
@@ -1082,14 +1092,14 @@ void RCOutput::dshot_send(pwm_group &group, bool blocking)
 
 
 /*
-  send a set of Neopixel packets for a channel group
+  send a set of Serial LED packets for a channel group
   return true if send was successful
  */
-bool RCOutput::neopixel_send(pwm_group &group)
+bool RCOutput::serial_led_send(pwm_group &group)
 {
 #ifndef DISABLE_DSHOT
     if (irq.waiter || !group.dma_handle->lock_nonblock()) {
-        // doing serial output, don't send Neopixel pulses
+        // doing serial output, don't send Serial LED pulses
         return false;
     }
 
@@ -1669,6 +1679,8 @@ uint32_t RCOutput::protocol_bitrate(const enum output_mode mode) const
         return 1200000;
     case MODE_NEOPIXEL:
         return 800000;
+    case MODE_PROFILED:
+        return 1500000; // experiment winding this up 3000000 max from data sheet
     default:
         // use 1 to prevent a possible divide-by-zero
         return 1;
@@ -1676,23 +1688,44 @@ uint32_t RCOutput::protocol_bitrate(const enum output_mode mode) const
 }
 
 /*
-  setup neopixel (WS2812B) output for a given channel number, with
+  setup serial led output for a given channel number, with
   the given max number of LEDs in the chain.
 */
-bool RCOutput::set_neopixel_num_LEDs(const uint16_t chan, uint8_t num_leds)
+bool RCOutput::set_serial_led_num_LEDs(const uint16_t chan, uint8_t num_leds, output_mode mode, uint16_t clock_mask)
 {
     uint8_t i;
     pwm_group *grp = find_chan(chan, i);
     if (!grp) {
         return false;
     }
-    if (grp->neopixel_nleds == num_leds && grp->current_mode == MODE_NEOPIXEL) {
-        // no change
-        return true;
+
+    switch (mode) {
+        case MODE_NEOPIXEL: {
+            grp->serial_nleds = MAX(num_leds, grp->serial_nleds);
+            break;
+        }
+        case MODE_PROFILED: {
+            // ProfiLED requires two dummy LED's to mark end of transmission
+            grp->serial_nleds = MAX(num_leds + 2, grp->serial_nleds);
+
+            // Enable any clock channels in the same group
+            grp->clock_mask = 0;
+            for (uint8_t j = 0; j < 4; j++) {
+                if ((clock_mask & (1U<<(grp->chan[j] + chan_offset))) != 0) {
+                    grp->clock_mask |= 1U<<j;
+                }
+            }
+
+            break;
+        }
+        default: {
+            return false;
+        }
     }
-    grp->neopixel_nleds = MAX(num_leds, grp->neopixel_nleds);
-    set_output_mode(1U<<chan, MODE_NEOPIXEL);
-    return grp->current_mode == MODE_NEOPIXEL;
+
+    set_output_mode(1U<<chan, mode);
+
+    return grp->current_mode == mode;
 }
 
 /*
@@ -1715,36 +1748,127 @@ void RCOutput::_set_neopixel_rgb_data(pwm_group *grp, uint8_t idx, uint8_t led,
 }
 
 /*
-  setup neopixel (WS2812B) output data for a given output channel
+  ProfiLED frame for a given output channel
+  channel is active high and bits inverted to get clock rising edge away from data rising edge
+*/
+void RCOutput::_set_profiled_rgb_data(pwm_group *grp, uint8_t idx, uint8_t led, uint8_t red, uint8_t green, uint8_t blue)
+{
+    const uint8_t pad_start_bits = 1;
+    const uint8_t bit_length = 25;
+    const uint8_t stride = 4;
+    uint32_t *buf = grp->dma_buffer + (led * bit_length + pad_start_bits) * stride + idx;
+    uint32_t bits = 0x1000000 | (blue<<16) | (red<<8) | green;
+    const uint32_t BIT_1 = 14 * grp->bit_width_mul;
+    for (uint16_t b=0; b < bit_length; b++) {
+        buf[b * stride] = (bits & 0x1000000) ? 0 : BIT_1;
+        bits <<= 1;
+    }
+}
+
+/*
+  ProfiLED blank frame for a given output channel
+  channel is active high and bits inverted to get clock rising edge away from data rising edge
+*/
+void RCOutput::_set_profiled_blank_frame(pwm_group *grp, uint8_t idx, uint8_t led)
+{
+    const uint8_t pad_start_bits = 1;
+    const uint8_t bit_length = 25;
+    const uint8_t stride = 4;
+    uint32_t *buf = grp->dma_buffer + (led * bit_length + pad_start_bits) * stride + idx;
+    const uint32_t BIT_1 = 14 * grp->bit_width_mul;
+    for (uint16_t b=0; b < bit_length; b++) {
+        buf[b * stride] = BIT_1;
+    }
+}
+
+/*
+  setup ProfiLED clock frame for a given output channel
+*/
+void RCOutput::_set_profiled_clock(pwm_group *grp, uint8_t idx, uint8_t led)
+{
+    const uint8_t pad_start_bits = 1;
+    const uint8_t bit_length = 25;
+    const uint8_t stride = 4;
+    uint32_t *buf = grp->dma_buffer + (led * bit_length + pad_start_bits) * stride + idx;
+    const uint32_t BIT_1 = 7 * grp->bit_width_mul;
+    for (uint16_t b=0; b < bit_length; b++) {
+        buf[b * stride] = BIT_1;
+    }
+}
+
+
+/*
+  setup serial LED output data for a given output channel
   and a LED number. LED -1 is all LEDs
 */
-void RCOutput::set_neopixel_rgb_data(const uint16_t chan, int8_t led, uint8_t red, uint8_t green, uint8_t blue)
+void RCOutput::set_serial_led_rgb_data(const uint16_t chan, int8_t led, uint8_t red, uint8_t green, uint8_t blue)
 {
     uint8_t i;
     pwm_group *grp = find_chan(chan, i);
     if (!grp) {
         return;
     }
-    if (led == -1) {
-        for (uint8_t n=0; n<grp->neopixel_nleds; n++) {
-            set_neopixel_rgb_data(chan, n, red, green, blue);
-        }
+
+    if (led >= grp->serial_nleds || (grp->current_mode != MODE_NEOPIXEL && grp->current_mode != MODE_PROFILED)) {
         return;
     }
 
-    if (led < -1 || led >= grp->neopixel_nleds) {
+    if (led == -1) {
+        grp->prepared_send = true;
+        for (uint8_t n=0; n<grp->serial_nleds; n++) {
+            set_serial_led_rgb_data(chan, n, red, green, blue);
+        }
         return;
+    } else if (!grp->prepared_send) {
+        // if not ouput clock and trailing frames, run through all LED's to do it now
+        set_serial_led_rgb_data(chan, -1, 0, 0, 0);
     }
 
-    _set_neopixel_rgb_data(grp, i, uint8_t(led), red, green, blue);
+    switch (grp->current_mode) {
+        case MODE_NEOPIXEL: {
+            _set_neopixel_rgb_data(grp, i, uint8_t(led), red, green, blue);
+            break;
+        }
+
+        case MODE_PROFILED: {
+            if (led < grp->serial_nleds - 2) {
+                _set_profiled_rgb_data(grp, i, uint8_t(led), red, green, blue);
+            } else {
+                _set_profiled_blank_frame(grp, i, uint8_t(led));
+            }
+
+            for (uint8_t j = 0; j < 4; j++) {
+                if ((grp->clock_mask & 1U<<j) != 0) {
+                   _set_profiled_clock(grp, j, uint8_t(led));
+                }
+            }
+
+            break;
+        }
+
+        default: {
+            return;
+        }
+    }
 }
 
 /*
-  trigger send of neopixel data
+  trigger send of serial led data for one group
 */
-void RCOutput::neopixel_send(void)
+void RCOutput::serial_led_send(const uint16_t chan)
 {
-    neopixel_pending = true;
+    uint8_t i;
+    pwm_group *grp = find_chan(chan, i);
+    if (!grp) {
+        return;
+    }
+    if (grp->current_mode != MODE_NEOPIXEL && grp->current_mode != MODE_PROFILED) {
+        return;
+    }
+    if (grp->prepared_send) {
+        serial_led_pending = true; 
+        grp->serial_led_pending = true;
+    }
 }
 
 #endif // HAL_USE_PWM

libraries/AP_HAL_ChibiOS/RCOutput.h

@@ -157,21 +157,21 @@ public:
     }
 
     /*
-      setup neopixel (WS2812B) output for a given channel number, with
+      setup serial LED output for a given channel number, with
       the given max number of LEDs in the chain.
      */
-    bool set_neopixel_num_LEDs(const uint16_t chan, uint8_t num_leds) override;
+    bool set_serial_led_num_LEDs(const uint16_t chan, uint8_t num_leds, output_mode mode = MODE_PWM_NONE, uint16_t clock_mask = 0) override;
 
     /*
-      setup neopixel (WS2812B) output data for a given output channel
+      setup serial LED output data for a given output channel
       and LEDs number. LED -1 is all LEDs
      */
-    void set_neopixel_rgb_data(const uint16_t chan, int8_t led, uint8_t red, uint8_t green, uint8_t blue) override;
+    void set_serial_led_rgb_data(const uint16_t chan, int8_t led, uint8_t red, uint8_t green, uint8_t blue) override;
 
     /*
-      trigger send of neopixel data
+      trigger send of serial LED data
      */
-    void neopixel_send(void) override;
+    void serial_led_send(const uint16_t chan) override;
 
 private:
     struct pwm_group {
@@ -201,7 +201,10 @@ private:
         bool in_serial_dma;
         uint64_t last_dmar_send_us;
         virtual_timer_t dma_timeout;
-        uint8_t neopixel_nleds;
+        uint8_t serial_nleds;
+        uint8_t clock_mask;
+        bool serial_led_pending;
+        bool prepared_send;
 
         // serial output
         struct {
@@ -336,11 +339,11 @@ private:
     uint16_t telem_request_mask;
 
     /*
-      NeoPixel handling. Max of 32 LEDs uses max 12k of memory per group
+      Serial lED handling. Max of 32 LEDs uses max 12k of memory per group
       return true if send was successful
     */
-    bool neopixel_send(pwm_group &group);
-    bool neopixel_pending;
+    bool serial_led_send(pwm_group &group);
+    bool serial_led_pending;
 
     void dma_allocate(Shared_DMA *ctx);
     void dma_deallocate(Shared_DMA *ctx);
@@ -358,10 +361,16 @@ private:
 
     /*
       setup neopixel (WS2812B) output data for a given output channel
-      and LEDs number. LED -1 is all LEDs
      */
     void _set_neopixel_rgb_data(pwm_group *grp, uint8_t idx, uint8_t led, uint8_t red, uint8_t green, uint8_t blue);
 
+    /*
+      setup ProfiLED output data for a given output channel
+     */
+    void _set_profiled_rgb_data(pwm_group *grp, uint8_t idx, uint8_t led, uint8_t red, uint8_t green, uint8_t blue);
+    void _set_profiled_clock(pwm_group *grp, uint8_t idx, uint8_t led);
+    void _set_profiled_blank_frame(pwm_group *grp, uint8_t idx, uint8_t led);
+
     // serial output support
     static const eventmask_t serial_event_mask = EVENT_MASK(1);
     bool serial_write_byte(uint8_t b);