mirror of https://github.com/ArduPilot/ardupilot
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
This commit is contained in:
parent
c17fffc20d
commit
175f4dfd4f
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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},
|
||||
|
|
|
@ -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
|
||||
|
|
Loading…
Reference in New Issue