From e35514b551ce2b74cff32403117f382daf0abbd0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 25 Jan 2022 20:58:35 +1100 Subject: [PATCH] HAL_ChibiOS: fixed neopixel control fixes frequency setup for neopixel, separating out DShot setup. Also allows for neopixel without other DMA controlled output types for LEDs in AP_Periph --- libraries/AP_HAL_ChibiOS/RCOutput.cpp | 33 +++++++++++++++++++-------- libraries/AP_HAL_ChibiOS/RCOutput.h | 3 ++- 2 files changed, 25 insertions(+), 11 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index 70029114c0..a9b7384d81 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -53,6 +53,7 @@ static const eventmask_t EVT_PWM_SEND = EVENT_MASK(11); static const eventmask_t EVT_PWM_START = EVENT_MASK(12); static const eventmask_t EVT_PWM_SYNTHETIC_SEND = EVENT_MASK(13); static const eventmask_t EVT_PWM_SEND_NEXT = EVENT_MASK(14); +static const eventmask_t EVT_LED_SEND = EVENT_MASK(15); // #pragma GCC optimize("Og") @@ -134,13 +135,13 @@ 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) { - chEvtWaitOne(EVT_PWM_SEND | EVT_PWM_SYNTHETIC_SEND); - + const auto mask = chEvtWaitOne(EVT_PWM_SEND | EVT_PWM_SYNTHETIC_SEND | EVT_LED_SEND); + const bool have_pwm_event = (mask & (EVT_PWM_SEND | EVT_PWM_SYNTHETIC_SEND)) != 0; // start the clock last_thread_run_us = AP_HAL::micros(); // this is when the cycle is supposed to start - if (_dshot_cycle == 0) { + if (_dshot_cycle == 0 && have_pwm_event) { last_cycle_run_us = AP_HAL::micros(); // register a timer for the next tick if push() will not be providing it if (_dshot_rate != 1) { @@ -157,7 +158,7 @@ void RCOutput::rcout_thread() // main thread requested a new dshot send or we timed out - if we are not running // as a multiple of loop rate then ignore EVT_PWM_SEND events to preserve periodicity - if (!serial_group) { + if (!serial_group && have_pwm_event) { dshot_send_groups(time_out_us); // now unlock everything @@ -715,7 +716,7 @@ void RCOutput::print_group_setup_error(pwm_group &group, const char* error_strin This is used for both DShot and serial output */ bool RCOutput::setup_group_DMA(pwm_group &group, uint32_t bitrate, uint32_t bit_width, bool active_high, const uint16_t buffer_length, bool choose_high, - uint32_t pulse_time_us) + uint32_t pulse_time_us, bool is_dshot) { #ifndef DISABLE_DSHOT // for dshot we setup for DMAR based output @@ -762,9 +763,20 @@ bool RCOutput::setup_group_DMA(pwm_group &group, uint32_t bitrate, uint32_t bit_ group.pwm_started = false; } - // original prescaler calculation from betaflight. bi-dir dshot is incredibly sensitive to the bitrate const uint32_t target_frequency = bitrate * bit_width; - uint32_t prescaler = uint32_t(lrintf((float) group.pwm_drv->clock / (bitrate * bit_width) + 0.01f) - 1); + uint32_t prescaler; + if (is_dshot) { + // original prescaler calculation from betaflight. bi-dir dshot is incredibly sensitive to the bitrate + prescaler = uint32_t(lrintf((float) group.pwm_drv->clock / (bitrate * bit_width) + 0.01f) - 1); + } else { + // adjust frequency to give an allowed value given the clock. + const uint32_t clock_hz = group.pwm_drv->clock; + prescaler = group.pwm_drv->clock / target_frequency; + while ((clock_hz / prescaler) * prescaler != clock_hz && prescaler <= 0x8000) { + prescaler++; + } + } + uint32_t freq = group.pwm_drv->clock / prescaler; if (freq > target_frequency && !choose_high) { prescaler++; @@ -862,7 +874,7 @@ void RCOutput::set_group_mode(pwm_group &group) // calculate min time between pulses taking into account the DMAR parallelism const uint32_t pulse_time_us = 1000000UL * bit_length / rate; - if (!setup_group_DMA(group, rate, bit_period, active_high, buffer_length, true, pulse_time_us)) { + if (!setup_group_DMA(group, rate, bit_period, active_high, buffer_length, true, pulse_time_us, false)) { group.current_mode = MODE_PWM_NONE; break; } @@ -879,7 +891,7 @@ void RCOutput::set_group_mode(pwm_group &group) // configure timer driver for DMAR at requested rate if (!setup_group_DMA(group, rate, bit_period, active_high, // choosing the low frequency for DSHOT150 is based on experimentation with BLHeli32 and bi-directional dshot - MAX(DSHOT_BUFFER_LENGTH, GCR_TELEMETRY_BUFFER_LEN), group.current_mode != MODE_PWM_DSHOT150, pulse_send_time_us)) { + MAX(DSHOT_BUFFER_LENGTH, GCR_TELEMETRY_BUFFER_LEN), group.current_mode != MODE_PWM_DSHOT150, pulse_send_time_us, true)) { group.current_mode = MODE_PWM_NORMAL; break; } @@ -1623,7 +1635,7 @@ bool RCOutput::serial_setup_output(uint8_t chan, uint32_t baudrate, uint16_t cha // channels in blheli pass-thru fast for (auto &group : pwm_group_list) { if (group.ch_mask & chanmask) { - if (!setup_group_DMA(group, baudrate, 10, false, DSHOT_BUFFER_LENGTH, false, 10)) { + if (!setup_group_DMA(group, baudrate, 10, false, DSHOT_BUFFER_LENGTH, false, 10, false)) { serial_end(); return false; } @@ -2333,6 +2345,7 @@ void RCOutput::serial_led_send(const uint16_t chan) } if (grp->prepared_send) { + chEvtSignal(rcout_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 a6f1534936..86d72866ab 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.h +++ b/libraries/AP_HAL_ChibiOS/RCOutput.h @@ -597,7 +597,8 @@ private: void dma_cancel(pwm_group& group); bool mode_requires_dma(enum output_mode mode) const; bool setup_group_DMA(pwm_group &group, uint32_t bitrate, uint32_t bit_width, bool active_high, - const uint16_t buffer_length, bool choose_high, uint32_t pulse_time_us); + const uint16_t buffer_length, bool choose_high, uint32_t pulse_time_us, + bool is_dshot); void send_pulses_DMAR(pwm_group &group, uint32_t buffer_length); void set_group_mode(pwm_group &group); static uint32_t protocol_bitrate(const enum output_mode mode);