diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index 0dba925e4d..45f0c62cf5 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -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_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") /* @@ -892,7 +904,6 @@ void RCOutput::set_group_mode(pwm_group &group) } const uint32_t rate = protocol_bitrate(group.current_mode); - const uint32_t bit_period = 20; // configure timer driver for DMAR at requested rate 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 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; break; } @@ -912,13 +923,12 @@ void RCOutput::set_group_mode(pwm_group &group) case MODE_PWM_DSHOT150 ... MODE_PWM_DSHOT1200: { const uint32_t rate = protocol_bitrate(group.current_mode); - const uint32_t bit_period = 20; bool active_high = is_bidir_dshot_enabled() ? false : true; // calculate min time between pulses const uint32_t pulse_send_time_us = 1000000UL * dshot_bit_length / 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)) { group.current_mode = MODE_PWM_NORMAL; 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) { - const uint32_t DSHOT_MOTOR_BIT_0 = 7 * clockmul; - const uint32_t DSHOT_MOTOR_BIT_1 = 14 * clockmul; + const uint32_t DSHOT_MOTOR_BIT_0 = DSHOT_BIT_0_TICKS * clockmul; + const uint32_t DSHOT_MOTOR_BIT_1 = DSHOT_BIT_1_TICKS * clockmul; uint16_t i = 0; for (; i < dshot_pre; i++) { 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; uint32_t *buf = grp->dma_buffer + (led * neopixel_bit_length + pad_start_bits) * stride + idx; uint32_t bits = (green<<16) | (red<<8) | blue; - const uint32_t BIT_0 = 7 * grp->bit_width_mul; - const uint32_t BIT_1 = 14 * grp->bit_width_mul; + const uint32_t BIT_0 = NEOP_BIT_0_TICKS * 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++) { buf[b * stride] = (bits & 0x800000) ? BIT_1 : BIT_0; 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; uint32_t *buf = grp->dma_buffer + (led * bit_length + pad_start_bits) * stride + idx; 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++) { buf[b * stride] = (bits & 0x1000000) ? 0 : BIT_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 stride = 4; 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++) { 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 stride = 4; 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++) { buf[b * stride] = BIT_1; } @@ -2382,7 +2392,14 @@ void RCOutput::timer_info(ExpandingString &str) str.printf("TIMERV1\n"); 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)); 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),