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

AP_HAL_ChibiOS: add support for @SYS/timers.txt

Browse Source 
move out prescaler calculation and use closest prescaler everywhere
include timer id in PWM groups
This commit is contained in:
Andy Piper 2022-02-10 14:42:33 +00:00 committed by Andrew Tridgell
parent ea1af70f2b
commit 83ac78e969
5 changed files with 41 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
libraries/AP_HAL_ChibiOS/RCOutput.cpp
View File

@ -24,6 +24,7 @@
#include "hwdef/common/watchdog.h"
#include <AP_InternalError/AP_InternalError.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
#include <AP_Common/ExpandingString.h>
#ifndef HAL_NO_UARTDRIVER
#include <GCS_MAVLink/GCS.h>
#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

9
libraries/AP_HAL_ChibiOS/RCOutput.h
View File

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

8
libraries/AP_HAL_ChibiOS/Util.cpp
View File

@ -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.

4
libraries/AP_HAL_ChibiOS/Util.h
View File

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

4
libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py
View File

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