Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 08:13:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

HAL_ChibiOS: support LEDs with a wider range of frequencies

Browse Source 
this fixed LEDs on FMUv5 boards on first 4 aux channels. We need to
round up not round down in the resulting bitrate
This commit is contained in:
Andrew Tridgell 2019-11-04 13:52:21 +11:00 committed by Randy Mackay
parent 146daf8cef
commit fc61cf3d3c
2 changed files with 33 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
libraries/AP_HAL_ChibiOS/RCOutput.cpp
View File

@ -498,7 +498,7 @@ bool RCOutput::mode_requires_dma(enum output_mode mode) const
This is used for both DShot and serial output 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 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)
{ {
#ifndef DISABLE_DSHOT #ifndef DISABLE_DSHOT
if (!group.dma_buffer || buffer_length != group.dma_buffer_len) { if (!group.dma_buffer || buffer_length != group.dma_buffer_len) {
@ -533,16 +533,35 @@ bool RCOutput::setup_group_DMA(pwm_group &group, uint32_t bitrate, uint32_t bit_
// adjust frequency to give an allowed value given the // adjust frequency to give an allowed value given the
// clock. There is probably a better way to do this // clock. There is probably a better way to do this
uint32_t clock_hz = group.pwm_drv->clock; const uint32_t original_bitrate = bitrate;
uint32_t freq = 0;
uint32_t target_frequency = bitrate * bit_width; uint32_t target_frequency = bitrate * bit_width;
uint32_t prescaler = clock_hz / target_frequency; while (true) {
while ((clock_hz / prescaler) * prescaler != clock_hz && prescaler <= 0x8000) { uint32_t clock_hz = group.pwm_drv->clock;
prescaler++; target_frequency = bitrate * bit_width;
} uint32_t prescaler = clock_hz / target_frequency;
uint32_t freq = clock_hz / prescaler; while ((clock_hz / prescaler) * prescaler != clock_hz && prescaler <= 0x8000) {
if (prescaler > 0x8000) { prescaler++;
group.dma_handle->unlock(); }
return false; freq = clock_hz / prescaler;
// hal.console->printf("CLOCK=%u FREQ=%u PRE=%u BR=%u\n", clock_hz, freq/bit_width, prescaler, bitrate);
if (prescaler > 0x8000) {
group.dma_handle->unlock();
return false;
}
if (!choose_high) {
break;
}
// we want to choose a frequency that gives at least the
// target, erring on the high side not low side
uint32_t actual_bitrate = freq / bit_width;
if (actual_bitrate >= original_bitrate || bitrate < 10000) {
break;
}
bitrate += 10000;
if (bitrate >= 2 * original_bitrate) {
break;
}
} }
group.pwm_cfg.frequency = freq; group.pwm_cfg.frequency = freq;
@ -613,7 +632,7 @@ void RCOutput::set_group_mode(pwm_group &group)
const uint8_t channels_per_group = 4; const uint8_t channels_per_group = 4;
const uint16_t neopixel_bit_length = bits_per_pixel * channels_per_group * group.neopixel_nleds + (pad_start_bits + pad_end_bits) * channels_per_group; const uint16_t neopixel_bit_length = bits_per_pixel * channels_per_group * group.neopixel_nleds + (pad_start_bits + pad_end_bits) * channels_per_group;
const uint16_t neopixel_buffer_length = neopixel_bit_length * sizeof(uint32_t); const uint16_t neopixel_buffer_length = neopixel_bit_length * sizeof(uint32_t);
if (!setup_group_DMA(group, rate, bit_period, true, neopixel_buffer_length)) { if (!setup_group_DMA(group, rate, bit_period, true, neopixel_buffer_length, true)) {
group.current_mode = MODE_PWM_NONE; group.current_mode = MODE_PWM_NONE;
break; break;
} }
@ -628,7 +647,7 @@ void RCOutput::set_group_mode(pwm_group &group)
const uint32_t bit_period = 20; const uint32_t bit_period = 20;
// configure timer driver for DMAR at requested rate // configure timer driver for DMAR at requested rate
if (!setup_group_DMA(group, rate, bit_period, true, dshot_buffer_length)) { if (!setup_group_DMA(group, rate, bit_period, true, dshot_buffer_length, false)) {
group.current_mode = MODE_PWM_NONE; group.current_mode = MODE_PWM_NONE;
break; break;
} }
@ -1135,7 +1154,7 @@ bool RCOutput::serial_setup_output(uint8_t chan, uint32_t baudrate, uint16_t cha
for (uint8_t i = 0; i < NUM_GROUPS; i++ ) { for (uint8_t i = 0; i < NUM_GROUPS; i++ ) {
pwm_group &group = pwm_group_list[i]; pwm_group &group = pwm_group_list[i];
if (group.ch_mask & chanmask) { if (group.ch_mask & chanmask) {
if (!setup_group_DMA(group, baudrate, 10, false, dshot_buffer_length)) { if (!setup_group_DMA(group, baudrate, 10, false, dshot_buffer_length, false)) {
serial_end(); serial_end();
return false; return false;
} }

2
libraries/AP_HAL_ChibiOS/RCOutput.h
View File

@ -348,7 +348,7 @@ private:
static void dma_irq_callback(void *p, uint32_t flags); static void dma_irq_callback(void *p, uint32_t flags);
static void dma_unlock(void *p); static void dma_unlock(void *p);
bool mode_requires_dma(enum output_mode mode) const; 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 setup_group_DMA(pwm_group &group, uint32_t bitrate, uint32_t bit_width, bool active_high, const uint16_t buffer_length, bool choose_high);
void send_pulses_DMAR(pwm_group &group, uint32_t buffer_length); void send_pulses_DMAR(pwm_group &group, uint32_t buffer_length);
void set_group_mode(pwm_group &group); void set_group_mode(pwm_group &group);
bool is_dshot_protocol(const enum output_mode mode) const; bool is_dshot_protocol(const enum output_mode mode) const;