aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

HAL_ChibiOS: fixed neopixel control

Browse Source 
fixes frequency setup for neopixel, separating out DShot setup. Also
allows for neopixel without other DMA controlled output types for LEDs
in AP_Periph
This commit is contained in:
Andrew Tridgell 2022-01-25 20:58:35 +11:00
parent c2dd7f4c9e
commit e35514b551
2 changed files with 25 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
libraries/AP_HAL_ChibiOS/RCOutput.cpp
View File

@ -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;
}

3
libraries/AP_HAL_ChibiOS/RCOutput.h
View File

@ -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);