AP_HAL_ChibiOS: move LED processing to a separate thread

LED processing on a separate thread allows much longer LED lengths to be handled without
compromising dshot timing or timeouts. The thread is also run at a lower priority to
reflect its lack of flight criticality

libraries/AP_HAL_ChibiOS/RCOutput.cpp

@@ -49,6 +49,14 @@ extern AP_IOMCU iomcu;
 #endif
 
 #define RCOU_SERIAL_TIMING_DEBUG 0
+#define LED_THD_WA_SIZE 256
+#ifndef HAL_NO_LED_THREAD
+#if defined(HAL_NO_RCOUT_THREAD)
+#define HAL_NO_LED_THREAD 1
+#else
+#define HAL_NO_LED_THREAD 0
+#endif
+#endif
 
 #define TELEM_IC_SAMPLE 16
 
@@ -145,6 +153,52 @@ void RCOutput::init()
     _initialised = true;
 }
 
+// start the led thread
+bool RCOutput::start_led_thread(void)
+{
+#if HAL_NO_LED_THREAD
+    return false;
+#else
+    WITH_SEMAPHORE(led_thread_sem);
+
+    if (led_thread_created) {
+        return true;
+    }
+
+    if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&RCOutput::led_thread, void), "led", LED_THD_WA_SIZE, AP_HAL::Scheduler::PRIORITY_LED, 0)) {
+        return false;
+    }
+
+    led_thread_created = true;
+    return true;
+#endif
+}
+
+/*
+  thread for handling LED RCOutpu
+ */
+void RCOutput::led_thread()
+{
+    {
+        WITH_SEMAPHORE(led_thread_sem);
+        led_thread_ctx = chThdGetSelfX();
+    }
+
+    // don't start outputting until fully configured
+    while (!hal.scheduler->is_system_initialized()) {
+        hal.scheduler->delay_microseconds(1000);
+    }
+
+    while (true) {
+        chEvtWaitOne(EVT_LED_SEND);
+        // if DMA sharing is in effect there can be quite a delay between the request to begin the cycle and
+        // actually sending out data - thus we need to work out how much time we have left to collect the locks
+
+        // process any pending LED output requests
+        led_timer_tick(LED_OUTPUT_PERIOD_US + AP_HAL::micros64());
+    }
+}
+
 /*
   thread for handling RCOutput send
  */
@@ -163,7 +217,7 @@ void RCOutput::rcout_thread()
     // dshot is quite sensitive to timing, it's important to output pulses as
     // regularly as possible at the correct bitrate
     while (true) {
-        const auto mask = chEvtWaitOne(EVT_PWM_SEND | EVT_PWM_SYNTHETIC_SEND | EVT_LED_SEND);
+        const auto mask = chEvtWaitOne(EVT_PWM_SEND | EVT_PWM_SYNTHETIC_SEND);
         const bool have_pwm_event = (mask & (EVT_PWM_SEND | EVT_PWM_SYNTHETIC_SEND)) != 0;
         // start the clock
         last_thread_run_us = AP_HAL::micros64();
@@ -226,13 +280,18 @@ __RAMFUNC__ void RCOutput::dshot_update_tick(void* p)
 
 #if AP_HAL_SHARED_DMA_ENABLED
 // release locks on the groups that are pending in reverse order
-void RCOutput::dshot_collect_dma_locks(uint64_t time_out_us)
+void RCOutput::dshot_collect_dma_locks(uint64_t time_out_us, bool led_thread)
 {
     if (NUM_GROUPS == 0) {
         return;
     }
     for (int8_t i = NUM_GROUPS - 1; i >= 0; i--) {
         pwm_group &group = pwm_group_list[i];
+
+        if ((led_thread && !is_led_protocol(group.current_mode)) || is_led_protocol(group.current_mode)) {
+            continue;
+        }
+
         if (group.dma_handle != nullptr && group.dma_handle->is_locked()) {
             // calculate how long we have left
             uint64_t now = AP_HAL::micros64();
@@ -252,7 +311,7 @@ void RCOutput::dshot_collect_dma_locks(uint64_t time_out_us)
             // timer wrap with ChibiOS timers. Use CH_CFG_ST_TIMEDELTA
             // as minimum. Don't allow for a very long delay (over _dshot_period_us)
             // to prevent bugs in handling timer wrap
-            const uint32_t max_delay_us = _dshot_period_us;
+            const uint32_t max_delay_us = led_thread ? LED_OUTPUT_PERIOD_US : _dshot_period_us;
             const uint32_t min_delay_us = 10; // matches our CH_CFG_ST_TIMEDELTA
             wait_us = constrain_uint32(wait_us, min_delay_us, max_delay_us);
             mask = chEvtWaitOneTimeout(group.dshot_event_mask, chTimeUS2I(wait_us));
@@ -686,7 +745,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 || group.current_mode == MODE_PROFILED) {
+                else if (is_dshot_protocol(group.current_mode) || is_led_protocol(group.current_mode)) {
                     // set period_us to time for pulse output, to enable very fast rates
                     period_us = group.dshot_pulse_time_us;
                 }
@@ -787,7 +846,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) || (mode == MODE_PROFILED);
+    return is_dshot_protocol(mode) || is_led_protocol(mode);
 #endif //#ifdef DISABLE_DSHOT
 }
 
@@ -945,6 +1004,11 @@ void RCOutput::set_group_mode(pwm_group &group)
         uint8_t bits_per_pixel = 24;
         bool active_high = true;
 
+        if (!start_led_thread()) {
+            group.current_mode = MODE_PWM_NONE;
+            break;
+        }
+
         if (group.current_mode == MODE_PROFILED) {
             bits_per_pixel = 25;
             active_high = false;
@@ -1226,17 +1290,6 @@ void RCOutput::timer_tick(uint64_t time_out_us)
         return;
     }
 
-    // if we have enough time left send out LED data
-    if (serial_led_pending && (time_out_us > (AP_HAL::micros64() + (_dshot_period_us >> 1)))) {
-        serial_led_pending = false;
-        for (auto &group : pwm_group_list) {
-            serial_led_pending |= !serial_led_send(group);
-        }
-
-        // release locks on the groups that are pending in reverse order
-        dshot_collect_dma_locks(time_out_us);
-    }
-
     if (min_pulse_trigger_us == 0) {
         return;
     }
@@ -1250,6 +1303,27 @@ void RCOutput::timer_tick(uint64_t time_out_us)
     }
 }
 
+/*
+  periodic timer called from led thread. This is used for LED output
+ */
+void RCOutput::led_timer_tick(uint64_t time_out_us)
+{
+    if (serial_group) {
+        return;
+    }
+
+    // if we have enough time left send out LED data
+    if (serial_led_pending && (time_out_us > (AP_HAL::micros64() + (LED_OUTPUT_PERIOD_US >> 1)))) {
+        serial_led_pending = false;
+        for (auto &group : pwm_group_list) {
+            serial_led_pending |= !serial_led_send(group);
+        }
+
+        // release locks on the groups that are pending in reverse order
+        dshot_collect_dma_locks(time_out_us, true);
+    }
+}
+
 // send dshot for all groups that support it
 void RCOutput::dshot_send_groups(uint64_t time_out_us)
 {
@@ -1525,11 +1599,11 @@ void RCOutput::dshot_send(pwm_group &group, uint64_t time_out_us)
 /*
   send a set of Serial LED packets for a channel group
   return true if send was successful
+  called from led thread
  */
 bool RCOutput::serial_led_send(pwm_group &group)
 {
-    if (!group.serial_led_pending
-        || (group.current_mode != MODE_NEOPIXEL && group.current_mode != MODE_PROFILED)) {
+    if (!group.serial_led_pending || !is_led_protocol(group.current_mode)) {
         return true;
     }
 
@@ -1549,7 +1623,7 @@ bool RCOutput::serial_led_send(pwm_group &group)
         fill_DMA_buffer_serial_led(group);
     }
 
-    group.dshot_waiter = rcout_thread_ctx;
+    group.dshot_waiter = led_thread_ctx;
 
     chEvtGetAndClearEvents(group.dshot_event_mask);
 
@@ -2176,7 +2250,7 @@ bool RCOutput::set_serial_led_num_LEDs(const uint16_t chan, uint8_t num_leds, ou
     }
 
     // we cant add more or change the type after the first setup
-    if (grp->current_mode == MODE_NEOPIXEL || grp->current_mode == MODE_PROFILED) {
+    if (is_led_protocol(grp->current_mode)) {
         return false;
     }
 
@@ -2349,7 +2423,7 @@ void RCOutput::set_serial_led_rgb_data(const uint16_t chan, int8_t led, uint8_t
         return;
     }
 
-    if ((grp->current_mode != grp->led_mode) && ((grp->led_mode == MODE_NEOPIXEL) || (grp->led_mode == MODE_PROFILED))) {
+    if ((grp->current_mode != grp->led_mode) && is_led_protocol(grp->led_mode)) {
         // Arrays have not yet been setup, do it now
         for (uint8_t j = 0; j < 4; j++) {
             delete[] grp->serial_led_data[j];
@@ -2377,7 +2451,7 @@ void RCOutput::set_serial_led_rgb_data(const uint16_t chan, int8_t led, uint8_t
             return;
         }
 
-    } else if ((grp->current_mode != MODE_NEOPIXEL) && (grp->current_mode != MODE_PROFILED)) {
+    } else if (!is_led_protocol(grp->current_mode)) {
         return;
     }
 
@@ -2426,6 +2500,10 @@ void RCOutput::serial_led_send(const uint16_t chan)
         return;
     }
 
+    if (led_thread_ctx == nullptr) {
+        return;
+    }
+
     uint8_t i;
     pwm_group *grp = find_chan(chan, i);
     if (!grp) {
@@ -2434,12 +2512,12 @@ void RCOutput::serial_led_send(const uint16_t chan)
 
     WITH_SEMAPHORE(grp->serial_led_mutex);
 
-    if (grp->serial_nleds == 0 || (grp->current_mode != MODE_NEOPIXEL && grp->current_mode != MODE_PROFILED)) {
+    if (grp->serial_nleds == 0 || !is_led_protocol(grp->current_mode)) {
         return;
     }
 
     if (grp->prepared_send) {
-        chEvtSignal(rcout_thread_ctx, EVT_LED_SEND);
+        chEvtSignal(led_thread_ctx, EVT_LED_SEND);
         grp->serial_led_pending = true;
         serial_led_pending = true;
     }

libraries/AP_HAL_ChibiOS/RCOutput.h

@@ -105,6 +105,11 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput
      */
     void timer_tick(uint64_t last_run_us);
 
+    /*
+      LED push
+     */
+    void led_timer_tick(uint64_t last_run_us);
+
     /*
       setup for serial output to a set of ESCs, using the given
       baudrate. Assumes 1 start bit, 1 stop bit, LSB first and 8
@@ -414,6 +419,17 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput
      */
     thread_t *rcout_thread_ctx;
 
+    /*
+      timer thread for use by led events
+     */
+    thread_t *led_thread_ctx;
+
+    /*
+      mutex to control LED thread creation
+     */
+    HAL_Semaphore led_thread_sem;
+    bool led_thread_created;
+
     /*
       structure for IRQ handler for soft-serial input
      */
@@ -583,6 +599,10 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput
     // update safety switch and LED
     void safety_update(void);
 
+    // LED thread
+    void led_thread();
+    bool start_led_thread();
+
     uint32_t telem_request_mask;
 
     /*
@@ -606,7 +626,7 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput
     static void dshot_update_tick(void* p);
     static void dshot_send_next_group(void* p);
     // release locks on the groups that are pending in reverse order
-    void dshot_collect_dma_locks(uint64_t last_run_us);
+    void dshot_collect_dma_locks(uint64_t last_run_us, bool led_thread = false);
     static void dma_up_irq_callback(void *p, uint32_t flags);
     static void dma_unlock(void *p);
     void dma_cancel(pwm_group& group);

libraries/AP_HAL_ChibiOS/Scheduler.cpp

@@ -650,6 +650,7 @@ uint8_t Scheduler::calculate_thread_priority(priority_base base, int8_t priority
         { PRIORITY_CAN, APM_CAN_PRIORITY},
         { PRIORITY_TIMER, APM_TIMER_PRIORITY},
         { PRIORITY_RCOUT, APM_RCOUT_PRIORITY},
+        { PRIORITY_LED, APM_LED_PRIORITY},
         { PRIORITY_RCIN, APM_RCIN_PRIORITY},
         { PRIORITY_IO, APM_IO_PRIORITY},
         { PRIORITY_UART, APM_UART_PRIORITY},

libraries/AP_HAL_ChibiOS/Scheduler.h

@@ -27,6 +27,7 @@
 #define APM_TIMER_PRIORITY      181
 #define APM_RCOUT_PRIORITY      181
 #define APM_RCIN_PRIORITY       177
+#define APM_LED_PRIORITY         60
 #define APM_UART_PRIORITY        60
 #define APM_UART_UNBUFFERED_PRIORITY 181
 #define APM_STORAGE_PRIORITY     59