Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_ChibiOS: use narrower bitwidths for dshot and LEDs to allow more accurate prescaler calculation

This commit is contained in:
Andy Piper 2022-03-08 18:03:55 +00:00 committed by Randy Mackay
parent 7552219c5b
commit 33c9ffd874
1 changed files with 29 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
libraries/AP_HAL_ChibiOS/RCOutput.cpp
View File

@ -59,6 +59,18 @@ 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_PWM_SEND_NEXT = EVENT_MASK(14);
static const eventmask_t EVT_LED_SEND = EVENT_MASK(15); static const eventmask_t EVT_LED_SEND = EVENT_MASK(15);
static const uint32_t DSHOT_BIT_WIDTH_TICKS = 8;
static const uint32_t DSHOT_BIT_0_TICKS = 3;
static const uint32_t DSHOT_BIT_1_TICKS = 6;
// See WS2812B spec for expected pulse widths
static const uint32_t NEOP_BIT_WIDTH_TICKS = 11;
static const uint32_t NEOP_BIT_0_TICKS = 4;
static const uint32_t NEOP_BIT_1_TICKS = 9;
// neopixel does not use pulse widths at all
static const uint32_t PROFI_BIT_0_TICKS = 4;
static const uint32_t PROFI_BIT_1_TICKS = 9;
// #pragma GCC optimize("Og") // #pragma GCC optimize("Og")
/* /*
@ -892,7 +904,6 @@ void RCOutput::set_group_mode(pwm_group &group)
} }
const uint32_t rate = protocol_bitrate(group.current_mode); const uint32_t rate = protocol_bitrate(group.current_mode);
const uint32_t bit_period = 20;
// configure timer driver for DMAR at requested rate // configure timer driver for DMAR at requested rate
const uint8_t pad_end_bits = 8; const uint8_t pad_end_bits = 8;
@ -903,7 +914,7 @@ void RCOutput::set_group_mode(pwm_group &group)
// calculate min time between pulses taking into account the DMAR parallelism // calculate min time between pulses taking into account the DMAR parallelism
const uint32_t pulse_time_us = 1000000UL * bit_length / rate; const uint32_t pulse_time_us = 1000000UL * bit_length / rate;
if (!setup_group_DMA(group, rate, bit_period, active_high, buffer_length, pulse_time_us, false)) { if (!setup_group_DMA(group, rate, NEOP_BIT_WIDTH_TICKS, active_high, buffer_length, pulse_time_us, false)) {
group.current_mode = MODE_PWM_NONE; group.current_mode = MODE_PWM_NONE;
break; break;
} }
@ -912,13 +923,12 @@ void RCOutput::set_group_mode(pwm_group &group)
case MODE_PWM_DSHOT150 ... MODE_PWM_DSHOT1200: { case MODE_PWM_DSHOT150 ... MODE_PWM_DSHOT1200: {
const uint32_t rate = protocol_bitrate(group.current_mode); const uint32_t rate = protocol_bitrate(group.current_mode);
const uint32_t bit_period = 20;
bool active_high = is_bidir_dshot_enabled() ? false : true; bool active_high = is_bidir_dshot_enabled() ? false : true;
// calculate min time between pulses // calculate min time between pulses
const uint32_t pulse_send_time_us = 1000000UL * dshot_bit_length / rate; const uint32_t pulse_send_time_us = 1000000UL * dshot_bit_length / rate;
// configure timer driver for DMAR at requested rate // configure timer driver for DMAR at requested rate
if (!setup_group_DMA(group, rate, bit_period, active_high, if (!setup_group_DMA(group, rate, DSHOT_BIT_WIDTH_TICKS, active_high,
MAX(DSHOT_BUFFER_LENGTH, GCR_TELEMETRY_BUFFER_LEN), pulse_send_time_us, true)) { MAX(DSHOT_BUFFER_LENGTH, GCR_TELEMETRY_BUFFER_LEN), pulse_send_time_us, true)) {
group.current_mode = MODE_PWM_NORMAL; group.current_mode = MODE_PWM_NORMAL;
break; break;
@ -1303,8 +1313,8 @@ uint16_t RCOutput::create_dshot_packet(const uint16_t value, bool telem_request,
*/ */
void RCOutput::fill_DMA_buffer_dshot(uint32_t *buffer, uint8_t stride, uint16_t packet, uint16_t clockmul) void RCOutput::fill_DMA_buffer_dshot(uint32_t *buffer, uint8_t stride, uint16_t packet, uint16_t clockmul)
{ {
const uint32_t DSHOT_MOTOR_BIT_0 = 7 * clockmul; const uint32_t DSHOT_MOTOR_BIT_0 = DSHOT_BIT_0_TICKS * clockmul;
const uint32_t DSHOT_MOTOR_BIT_1 = 14 * clockmul; const uint32_t DSHOT_MOTOR_BIT_1 = DSHOT_BIT_1_TICKS * clockmul;
uint16_t i = 0; uint16_t i = 0;
for (; i < dshot_pre; i++) { for (; i < dshot_pre; i++) {
buffer[i * stride] = 0; buffer[i * stride] = 0;
@ -2193,8 +2203,8 @@ void RCOutput::_set_neopixel_rgb_data(pwm_group *grp, uint8_t idx, uint8_t led,
const uint8_t stride = 4; const uint8_t stride = 4;
uint32_t *buf = grp->dma_buffer + (led * neopixel_bit_length + pad_start_bits) * stride + idx; uint32_t *buf = grp->dma_buffer + (led * neopixel_bit_length + pad_start_bits) * stride + idx;
uint32_t bits = (green<<16) | (red<<8) | blue; uint32_t bits = (green<<16) | (red<<8) | blue;
const uint32_t BIT_0 = 7 * grp->bit_width_mul; const uint32_t BIT_0 = NEOP_BIT_0_TICKS * grp->bit_width_mul;
const uint32_t BIT_1 = 14 * grp->bit_width_mul; const uint32_t BIT_1 = NEOP_BIT_1_TICKS * grp->bit_width_mul;
for (uint16_t b=0; b < 24; b++) { for (uint16_t b=0; b < 24; b++) {
buf[b * stride] = (bits & 0x800000) ? BIT_1 : BIT_0; buf[b * stride] = (bits & 0x800000) ? BIT_1 : BIT_0;
bits <<= 1; bits <<= 1;
@ -2212,7 +2222,7 @@ void RCOutput::_set_profiled_rgb_data(pwm_group *grp, uint8_t idx, uint8_t led,
const uint8_t stride = 4; const uint8_t stride = 4;
uint32_t *buf = grp->dma_buffer + (led * bit_length + pad_start_bits) * stride + idx; uint32_t *buf = grp->dma_buffer + (led * bit_length + pad_start_bits) * stride + idx;
uint32_t bits = 0x1000000 | (blue<<16) | (red<<8) | green; uint32_t bits = 0x1000000 | (blue<<16) | (red<<8) | green;
const uint32_t BIT_1 = 14 * grp->bit_width_mul; const uint32_t BIT_1 = PROFI_BIT_1_TICKS * grp->bit_width_mul;
for (uint16_t b=0; b < bit_length; b++) { for (uint16_t b=0; b < bit_length; b++) {
buf[b * stride] = (bits & 0x1000000) ? 0 : BIT_1; buf[b * stride] = (bits & 0x1000000) ? 0 : BIT_1;
bits <<= 1; bits <<= 1;
@ -2229,7 +2239,7 @@ void RCOutput::_set_profiled_blank_frame(pwm_group *grp, uint8_t idx, uint8_t le
const uint8_t bit_length = 25; const uint8_t bit_length = 25;
const uint8_t stride = 4; const uint8_t stride = 4;
uint32_t *buf = grp->dma_buffer + (led * bit_length + pad_start_bits) * stride + idx; uint32_t *buf = grp->dma_buffer + (led * bit_length + pad_start_bits) * stride + idx;
const uint32_t BIT_1 = 14 * grp->bit_width_mul; const uint32_t BIT_1 = PROFI_BIT_1_TICKS * grp->bit_width_mul;
for (uint16_t b=0; b < bit_length; b++) { for (uint16_t b=0; b < bit_length; b++) {
buf[b * stride] = BIT_1; buf[b * stride] = BIT_1;
} }
@ -2244,7 +2254,7 @@ void RCOutput::_set_profiled_clock(pwm_group *grp, uint8_t idx, uint8_t led)
const uint8_t bit_length = 25; const uint8_t bit_length = 25;
const uint8_t stride = 4; const uint8_t stride = 4;
uint32_t *buf = grp->dma_buffer + (led * bit_length + pad_start_bits) * stride + idx; uint32_t *buf = grp->dma_buffer + (led * bit_length + pad_start_bits) * stride + idx;
const uint32_t BIT_1 = 7 * grp->bit_width_mul; const uint32_t BIT_1 = PROFI_BIT_0_TICKS * grp->bit_width_mul;
for (uint16_t b=0; b < bit_length; b++) { for (uint16_t b=0; b < bit_length; b++) {
buf[b * stride] = BIT_1; buf[b * stride] = BIT_1;
} }
@ -2382,7 +2392,14 @@ void RCOutput::timer_info(ExpandingString &str)
str.printf("TIMERV1\n"); str.printf("TIMERV1\n");
for (auto &group : pwm_group_list) { for (auto &group : pwm_group_list) {
const uint32_t target_freq = &group == serial_group ? 19200 * 10 : protocol_bitrate(group.current_mode) * 20; uint32_t target_freq;
if (&group == serial_group) {
target_freq = 19200 * 10;
} else if (is_dshot_protocol(group.current_mode)) {
target_freq = protocol_bitrate(group.current_mode) * DSHOT_BIT_WIDTH_TICKS;
} else {
target_freq = protocol_bitrate(group.current_mode) * NEOP_BIT_WIDTH_TICKS;
}
const uint32_t prescaler = calculate_bitrate_prescaler(group.pwm_drv->clock, target_freq, is_dshot_protocol(group.current_mode)); const uint32_t prescaler = calculate_bitrate_prescaler(group.pwm_drv->clock, target_freq, is_dshot_protocol(group.current_mode));
str.printf("TIM%-2u CLK=%4uMhz MODE=%5s FREQ=%8u TGT=%8u\n", group.timer_id, unsigned(group.pwm_drv->clock / 1000000), str.printf("TIM%-2u CLK=%4uMhz MODE=%5s FREQ=%8u TGT=%8u\n", group.timer_id, unsigned(group.pwm_drv->clock / 1000000),
get_output_mode_string(group.current_mode), get_output_mode_string(group.current_mode),