From b2be3c1dbcd1fb419f249d4d9480830ba6161213 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 23 Feb 2023 12:35:34 +0000 Subject: [PATCH] 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 | 126 ++++++++++++++++++++----- libraries/AP_HAL_ChibiOS/RCOutput.h | 22 ++++- libraries/AP_HAL_ChibiOS/Scheduler.cpp | 1 + libraries/AP_HAL_ChibiOS/Scheduler.h | 1 + 4 files changed, 125 insertions(+), 25 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index 5d81171f53..c0042e7e7a 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/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; } diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.h b/libraries/AP_HAL_ChibiOS/RCOutput.h index 6ee05e85cd..c451ed3587 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.h +++ b/libraries/AP_HAL_ChibiOS/RCOutput.h @@ -104,6 +104,11 @@ public: */ 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 @@ -413,6 +418,17 @@ private: */ 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 */ @@ -582,6 +598,10 @@ private: // update safety switch and LED void safety_update(void); + // LED thread + void led_thread(); + bool start_led_thread(); + uint32_t telem_request_mask; /* @@ -605,7 +625,7 @@ private: 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); diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.cpp b/libraries/AP_HAL_ChibiOS/Scheduler.cpp index 7d4bf11c8f..a30a1da492 100644 --- a/libraries/AP_HAL_ChibiOS/Scheduler.cpp +++ b/libraries/AP_HAL_ChibiOS/Scheduler.cpp @@ -662,6 +662,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}, diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.h b/libraries/AP_HAL_ChibiOS/Scheduler.h index 95a9ce4d55..2a7187e86c 100644 --- a/libraries/AP_HAL_ChibiOS/Scheduler.h +++ b/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