diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index 306b76ed3b..8a593ecb54 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -24,6 +24,7 @@ #include "hwdef/common/watchdog.h" #include #include +#include #ifndef HAL_NO_UARTDRIVER #include #endif @@ -719,7 +720,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, +bool RCOutput::setup_group_DMA(pwm_group &group, uint32_t bitrate, uint32_t bit_width, bool active_high, const uint16_t buffer_length, uint32_t pulse_time_us, bool is_dshot) { #ifndef DISABLE_DSHOT @@ -768,32 +769,15 @@ bool RCOutput::setup_group_DMA(pwm_group &group, uint32_t bitrate, uint32_t bit_ } const uint32_t target_frequency = bitrate * bit_width; - 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, erring on the high side - const uint32_t clock_hz = group.pwm_drv->clock; - prescaler = group.pwm_drv->clock / target_frequency; - while ((clock_hz / prescaler) < target_frequency && prescaler > 1) { - prescaler--; - } - } + const uint32_t prescaler = calculate_bitrate_prescaler(group.pwm_drv->clock, target_frequency, is_dshot); - uint32_t freq = group.pwm_drv->clock / prescaler; - if (freq > target_frequency && !choose_high) { - prescaler++; - } else if (freq < target_frequency && choose_high) { - prescaler--; - } if (prescaler > 0x8000) { group.dma_handle->unlock(); print_group_setup_error(group, "failed to match clock speed"); return false; } - freq = group.pwm_drv->clock / prescaler; + const uint32_t freq = group.pwm_drv->clock / prescaler; group.pwm_cfg.frequency = freq; group.pwm_cfg.period = bit_width; group.pwm_cfg.dier = TIM_DIER_UDE; @@ -878,7 +862,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, false)) { + if (!setup_group_DMA(group, rate, bit_period, active_high, buffer_length, pulse_time_us, false)) { group.current_mode = MODE_PWM_NONE; break; } @@ -894,8 +878,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, true)) { + MAX(DSHOT_BUFFER_LENGTH, GCR_TELEMETRY_BUFFER_LEN), pulse_send_time_us, true)) { group.current_mode = MODE_PWM_NORMAL; break; } @@ -1639,7 +1622,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, false)) { + if (!setup_group_DMA(group, baudrate, 10, false, DSHOT_BUFFER_LENGTH, 10, false)) { serial_end(); return false; } @@ -2350,4 +2333,18 @@ void RCOutput::serial_led_send(const uint16_t chan) } } +void RCOutput::timer_info(ExpandingString &str) +{ + // a header to allow for machine parsers to determine format + 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; + 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), + unsigned(group.pwm_drv->clock / prescaler), unsigned(target_freq)); + } +} + #endif // HAL_USE_PWM diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.h b/libraries/AP_HAL_ChibiOS/RCOutput.h index 86d72866ab..39708a11c0 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.h +++ b/libraries/AP_HAL_ChibiOS/RCOutput.h @@ -248,6 +248,11 @@ public: */ void rcout_thread(); + /* + timer information + */ + void timer_info(ExpandingString &str) override; + private: enum class DshotState { IDLE = 0, @@ -282,6 +287,7 @@ private: uint8_t chan[4]; // chan number, zero based, 255 for disabled PWMConfig pwm_cfg; PWMDriver* pwm_drv; + uint8_t timer_id; bool have_up_dma; // can we do DMAR outputs for DShot? uint8_t dma_up_stream_id; uint8_t dma_up_channel; @@ -294,7 +300,6 @@ private: #endif uint8_t alt_functions[4]; ioline_t pal_lines[4]; - // below this line is not initialised by hwdef.h enum output_mode current_mode; uint16_t frequency_hz; @@ -597,7 +602,7 @@ 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, 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); diff --git a/libraries/AP_HAL_ChibiOS/Util.cpp b/libraries/AP_HAL_ChibiOS/Util.cpp index 8017bd5b91..7a9374a53c 100644 --- a/libraries/AP_HAL_ChibiOS/Util.cpp +++ b/libraries/AP_HAL_ChibiOS/Util.cpp @@ -606,6 +606,14 @@ void Util::uart_info(ExpandingString &str) } #endif +// request information on uart I/O +#if HAL_USE_PWM == TRUE +void Util::timer_info(ExpandingString &str) +{ + hal.rcout->timer_info(str); +} +#endif + /** * This method will generate random values with set size. It will fall back to AP_Math's get_random16() * if True RNG fails or enough entropy is not present. diff --git a/libraries/AP_HAL_ChibiOS/Util.h b/libraries/AP_HAL_ChibiOS/Util.h index d76b6b02ee..7ca209f882 100644 --- a/libraries/AP_HAL_ChibiOS/Util.h +++ b/libraries/AP_HAL_ChibiOS/Util.h @@ -91,7 +91,9 @@ public: // request information on uart I/O virtual void uart_info(ExpandingString &str) override; #endif - +#if HAL_USE_PWM == TRUE + void timer_info(ExpandingString &str) override; +#endif // returns random values bool get_random_vals(uint8_t* data, size_t size) override; diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index 5a7dfcff13..920ac60f3f 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -1986,7 +1986,7 @@ def write_PWM_config(f, ordered_timers): {%s, NULL}, \\ {%s, NULL}, \\ {%s, NULL} \\ - }, 0, 0}, &PWMD%u, \\ + }, 0, 0}, &PWMD%u, %u, \\ HAL_PWM%u_DMA_CONFIG, \\%s { %u, %u, %u, %u }, \\ { %s, %s, %s, %s }}\n''' % @@ -1994,7 +1994,7 @@ def write_PWM_config(f, ordered_timers): chan_list[0], chan_list[1], chan_list[2], chan_list[3], pwm_clock, period, chan_mode[0], chan_mode[1], chan_mode[2], chan_mode[3], - n, n, hal_icu_cfg, + n, n, n, hal_icu_cfg, alt_functions[0], alt_functions[1], alt_functions[2], alt_functions[3], pal_lines[0], pal_lines[1], pal_lines[2], pal_lines[3])) f.write('#define HAL_PWM_GROUPS %s\n\n' % ','.join(groups))