Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 08:38:36 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_ChibiOS: cache values of io_dshot() and io_enabled()

Browse Source 
enabled shared_up_dma to be fully compiled out
address some minor review comments
This commit is contained in:
Andy Piper 2023-10-19 16:22:08 +01:00 committed by Andrew Tridgell
parent 153c5181cb
commit 8c03c9e4bf
8 changed files with 90 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
libraries/AP_HAL_ChibiOS/RCOutput.cpp
View File

@ -100,6 +100,10 @@ void RCOutput::init()
if (AP_BoardConfig::io_enabled()) {
// with IOMCU the local (FMU) channels start at 8
chan_offset = 8;
iomcu_enabled = true;
}
if (AP_BoardConfig::io_dshot()) {
iomcu_dshot = true;
}
#endif
@ -144,7 +148,7 @@ void RCOutput::init()
}
#if HAL_WITH_IO_MCU
if (AP_BoardConfig::io_enabled()) {
if (iomcu_enabled) {
iomcu.init();
}
#endif
@ -442,7 +446,7 @@ void RCOutput::set_freq_group(pwm_group &group)
void RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
{
#if HAL_WITH_IO_MCU
if (AP_BoardConfig::io_enabled()) {
if (iomcu_enabled) {
// change frequency on IOMCU
uint16_t io_chmask = chmask & 0xFF;
if (io_chmask) {
@ -499,7 +503,7 @@ void RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
void RCOutput::set_default_rate(uint16_t freq_hz)
{
#if HAL_WITH_IO_MCU
if (AP_BoardConfig::io_enabled()) {
if (iomcu_enabled) {
iomcu.set_default_rate(freq_hz);
}
#endif
@ -526,7 +530,7 @@ void RCOutput::set_dshot_rate(uint8_t dshot_rate, uint16_t loop_rate_hz)
_dshot_period_us = 1000UL;
_dshot_rate = 0;
#if HAL_WITH_IO_MCU
if (AP_BoardConfig::io_dshot()) {
if (iomcu_dshot) {
iomcu.set_dshot_period(1000UL, 0);
}
#endif
@ -542,7 +546,7 @@ void RCOutput::set_dshot_rate(uint8_t dshot_rate, uint16_t loop_rate_hz)
#if HAL_WITH_IO_MCU
// this is not strictly neccessary since the iomcu could run at a different rate,
// but there is only one parameter to control this
if (AP_BoardConfig::io_dshot()) {
if (iomcu_dshot) {
iomcu.set_dshot_period(1000UL, 0);
}
#endif
@ -566,7 +570,7 @@ void RCOutput::set_dshot_rate(uint8_t dshot_rate, uint16_t loop_rate_hz)
}
_dshot_period_us = 1000000UL / drate;
#if HAL_WITH_IO_MCU
if (AP_BoardConfig::io_dshot()) {
if (iomcu_dshot) {
iomcu.set_dshot_period(_dshot_period_us, _dshot_rate);
}
#endif
@ -593,7 +597,7 @@ void RCOutput::set_dshot_esc_type(DshotEscType dshot_esc_type)
break;
}
#if HAL_WITH_IO_MCU
if (AP_BoardConfig::io_dshot()) {
if (iomcu_dshot) {
iomcu.set_dshot_esc_type(dshot_esc_type);
}
#endif
@ -670,7 +674,7 @@ uint16_t RCOutput::get_freq(uint8_t chan)
void RCOutput::enable_ch(uint8_t chan)
{
#if HAL_WITH_IO_MCU
if (chan < chan_offset && AP_BoardConfig::io_enabled()) {
if (chan < chan_offset && iomcu_enabled) {
iomcu.enable_ch(chan);
return;
}
@ -686,7 +690,7 @@ void RCOutput::enable_ch(uint8_t chan)
void RCOutput::disable_ch(uint8_t chan)
{
#if HAL_WITH_IO_MCU
if (chan < chan_offset && AP_BoardConfig::io_enabled()) {
if (chan < chan_offset && iomcu_enabled) {
iomcu.disable_ch(chan);
return;
}
@ -717,7 +721,7 @@ void RCOutput::write(uint8_t chan, uint16_t period_us)
#if HAL_WITH_IO_MCU
// handle IO MCU channels
if (AP_BoardConfig::io_enabled()) {
if (iomcu_enabled) {
iomcu.write_channel(chan, period_us);
}
#endif
@ -1198,7 +1202,7 @@ void RCOutput::set_output_mode(uint32_t mask, const enum output_mode mode)
mode == MODE_PWM_BRUSHED ||
(mode >= MODE_PWM_DSHOT150 && mode <= MODE_PWM_DSHOT600)) &&
iomcu_mask &&
AP_BoardConfig::io_enabled()) {
iomcu_enabled) {
iomcu.set_output_mode(iomcu_mask, mode);
return;
}
@ -1234,7 +1238,7 @@ RCOutput::output_mode RCOutput::get_output_mode(uint32_t& mask)
void RCOutput::set_telem_request_mask(uint32_t mask)
{
#if HAL_WITH_IO_MCU
if (AP_BoardConfig::io_dshot() && (mask & ((1U<<chan_offset)-1))) {
if (iomcu_dshot && (mask & ((1U<<chan_offset)-1))) {
iomcu.set_telem_request_mask(mask);
}
#endif
@ -1257,7 +1261,7 @@ bool RCOutput::get_output_mode_banner(char banner_msg[], uint8_t banner_msg_len)
#if HAL_WITH_IO_MCU
// fill in ch_mode array for IOMCU channels
if (AP_BoardConfig::io_enabled()) {
if (iomcu_enabled) {
uint8_t iomcu_mask;
const output_mode iomcu_mode = iomcu.get_output_mode(iomcu_mask);
for (uint8_t i = 0; i < chan_offset; i++ ) {
@ -1320,7 +1324,7 @@ void RCOutput::cork(void)
{
corked = true;
#if HAL_WITH_IO_MCU
if (AP_BoardConfig::io_enabled()) {
if (iomcu_enabled) {
iomcu.cork();
}
#endif
@ -1334,7 +1338,7 @@ void RCOutput::push(void)
corked = false;
push_local();
#if HAL_WITH_IO_MCU
if (AP_BoardConfig::io_enabled()) {
if (iomcu_enabled) {
iomcu.push();
}
#endif
@ -1346,7 +1350,7 @@ void RCOutput::push(void)
bool RCOutput::enable_px4io_sbus_out(uint16_t rate_hz)
{
#if HAL_WITH_IO_MCU
if (AP_BoardConfig::io_enabled()) {
if (iomcu_enabled) {
return iomcu.enable_sbus_out(rate_hz);
}
#endif
@ -1469,7 +1473,7 @@ void RCOutput::dshot_send_groups(uint64_t time_out_us)
dshot_send(group, time_out_us);
pulse_sent = true;
}
#ifdef HAL_WITH_BIDIR_DSHOT
#if defined(HAL_WITH_BIDIR_DSHOT) && defined(HAL_TIM_UP_SHARED)
// prevent the next send going out until the previous send has released its DMA channel
if (pulse_sent && group.shared_up_dma && group.bdshot.enabled) {
chEvtWaitOneTimeout(DSHOT_CASCADE, calc_ticks_remaining(group, time_out_us, _dshot_period_us));
@ -1500,9 +1504,9 @@ __RAMFUNC__ void RCOutput::dshot_send_next_group(void* p)
#if AP_HAL_SHARED_DMA_ENABLED
void RCOutput::dma_allocate(Shared_DMA *ctx)
{
chSysLock();
for (auto &group : pwm_group_list) {
if (group.dma_handle == ctx && group.dma == nullptr) {
chSysLock();
group.dma = dmaStreamAllocI(group.dma_up_stream_id, 10, dma_up_irq_callback, &group);
#if defined(STM32F1)
if (group.pwm_started && group.dma_handle->is_shared()) {
@ -1516,9 +1520,9 @@ void RCOutput::dma_allocate(Shared_DMA *ctx)
dmaSetRequestSource(group.dma, group.dma_up_channel);
}
#endif
}
}
chSysUnlock();
}
}
}
/*
@ -1526,9 +1530,9 @@ void RCOutput::dma_allocate(Shared_DMA *ctx)
*/
void RCOutput::dma_deallocate(Shared_DMA *ctx)
{
chSysLock();
for (auto &group : pwm_group_list) {
if (group.dma_handle == ctx && group.dma != nullptr) {
chSysLock();
#if defined(STM32F1)
// leaving the peripheral running on IOMCU plays havoc with the UART that is
// also sharing this channel, we only turn it off rather than resetting so
@ -1540,9 +1544,9 @@ void RCOutput::dma_deallocate(Shared_DMA *ctx)
#endif
dmaStreamFreeI(group.dma);
group.dma = nullptr;
}
}
chSysUnlock();
}
}
}
#endif // AP_HAL_SHARED_DMA_ENABLED
@ -2229,7 +2233,7 @@ void RCOutput::serial_end(void)
AP_HAL::Util::safety_state RCOutput::_safety_switch_state(void)
{
#if HAL_WITH_IO_MCU
if (AP_BoardConfig::io_enabled()) {
if (iomcu_enabled) {
safety_state = iomcu.get_safety_switch_state();
}
#endif
@ -2245,7 +2249,7 @@ AP_HAL::Util::safety_state RCOutput::_safety_switch_state(void)
bool RCOutput::force_safety_on(void)
{
#if HAL_WITH_IO_MCU
if (AP_BoardConfig::io_enabled()) {
if (iomcu_enabled) {
return iomcu.force_safety_on();
}
#endif
@ -2259,7 +2263,7 @@ bool RCOutput::force_safety_on(void)
void RCOutput::force_safety_off(void)
{
#if HAL_WITH_IO_MCU
if (AP_BoardConfig::io_enabled()) {
if (iomcu_enabled) {
iomcu.force_safety_off();
return;
}
@ -2327,7 +2331,7 @@ void RCOutput::safety_update(void)
void RCOutput::set_failsafe_pwm(uint32_t chmask, uint16_t period_us)
{
#if HAL_WITH_IO_MCU
if (AP_BoardConfig::io_enabled()) {
if (iomcu_enabled) {
iomcu.set_failsafe_pwm(chmask, period_us);
}
#endif

13
libraries/AP_HAL_ChibiOS/RCOutput.h
View File

@ -331,7 +331,9 @@ private:
uint8_t stream_id;
uint8_t channel;
} dma_ch[4];
#ifdef HAL_TIM_UP_SHARED
bool shared_up_dma; // do we need to wait for TIMx_UP DMA to be finished after use
#endif
#endif
uint8_t alt_functions[4];
ioline_t pal_lines[4];
@ -517,6 +519,13 @@ private:
static pwm_group pwm_group_list[];
static const uint8_t NUM_GROUPS;
#if HAL_WITH_IO_MCU
// cached values of AP_BoardConfig::io_enabled() and AP_BoardConfig::io_dshot()
// in case the user changes them
bool iomcu_enabled;
bool iomcu_dshot;
#endif
// offset of first local channel
uint8_t chan_offset;
@ -668,7 +677,11 @@ private:
void fill_DMA_buffer_dshot(dmar_uint_t *buffer, uint8_t stride, uint16_t packet, uint16_t clockmul);
// event to allow dshot cascading
#if defined(HAL_TIM_UP_SHARED)
static const eventmask_t DSHOT_CASCADE = EVENT_MASK(16);
#else
static const eventmask_t DSHOT_CASCADE = 0;
#endif
static const eventmask_t EVT_PWM_SEND = EVENT_MASK(11);
static const eventmask_t EVT_PWM_SYNTHETIC_SEND = EVENT_MASK(13);

29
libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp
View File

@ -54,7 +54,7 @@ void RCOutput::set_bidir_dshot_mask(uint32_t mask)
{
#if HAL_WITH_IO_MCU
const uint32_t iomcu_mask = ((1U<<chan_offset)-1);
if (AP_BoardConfig::io_dshot() && (mask & iomcu_mask)) {
if (iomcu_dshot && (mask & iomcu_mask)) {
iomcu.set_bidir_dshot_mask(mask & iomcu_mask);
}
#endif
@ -188,21 +188,21 @@ bool RCOutput::bdshot_setup_group_ic_DMA(pwm_group &group)
*/
void RCOutput::bdshot_ic_dma_allocate(Shared_DMA *ctx)
{
chSysLock();
for (uint8_t i = 0; i < NUM_GROUPS; i++ ) {
pwm_group &group = pwm_group_list[i];
for (uint8_t icuch = 0; icuch < 4; icuch++) {
if (group.bdshot.ic_dma_handle[icuch] == ctx && group.bdshot.ic_dma[icuch] == nullptr) {
chSysLock();
group.bdshot.ic_dma[icuch] = dmaStreamAllocI(group.dma_ch[icuch].stream_id, 10, bdshot_dma_ic_irq_callback, &group);
#if STM32_DMA_SUPPORTS_DMAMUX
if (group.bdshot.ic_dma[icuch]) {
dmaSetRequestSource(group.bdshot.ic_dma[icuch], group.dma_ch[icuch].channel);
}
#endif
}
}
}
chSysUnlock();
}
}
}
}
/*
@ -210,17 +210,17 @@ void RCOutput::bdshot_ic_dma_allocate(Shared_DMA *ctx)
*/
void RCOutput::bdshot_ic_dma_deallocate(Shared_DMA *ctx)
{
chSysLock();
for (uint8_t i = 0; i < NUM_GROUPS; i++ ) {
pwm_group &group = pwm_group_list[i];
for (uint8_t icuch = 0; icuch < 4; icuch++) {
chSysLock();
if (group.bdshot.ic_dma_handle[icuch] == ctx && group.bdshot.ic_dma[icuch] != nullptr) {
dmaStreamFreeI(group.bdshot.ic_dma[icuch]);
group.bdshot.ic_dma[icuch] = nullptr;
}
}
}
chSysUnlock();
}
}
}
// setup bdshot for sending and receiving the next pulse
@ -238,7 +238,11 @@ void RCOutput::bdshot_prepare_for_next_pulse(pwm_group& group)
} else {
osalDbgAssert(!group.bdshot.curr_ic_dma_handle, "IC DMA handle has not been released");
group.bdshot.curr_ic_dma_handle = group.bdshot.ic_dma_handle[group.bdshot.curr_telem_chan];
#ifdef HAL_TIM_UP_SHARED
osalDbgAssert(group.shared_up_dma || !group.bdshot.curr_ic_dma_handle->is_locked(), "IC DMA handle is already locked");
#else
osalDbgAssert(!group.bdshot.curr_ic_dma_handle->is_locked(), "IC DMA handle is already locked");
#endif
group.bdshot.curr_ic_dma_handle->lock();
group.bdshot.enabled = true;
}
@ -461,7 +465,7 @@ void RCOutput::bdshot_config_icu_dshot(stm32_tim_t* TIMx, uint8_t chan, uint8_t
break;
}
}
#endif
#endif // !defined(STM32F1)
/*
unlock DMA channel after a bi-directional dshot transaction completes
@ -488,13 +492,14 @@ __RAMFUNC__ void RCOutput::bdshot_finish_dshot_gcr_transaction(virtual_timer_t*
stm32_cacheBufferInvalidate(group->dma_buffer, group->bdshot.dma_tx_size);
memcpy(group->bdshot.dma_buffer_copy, group->dma_buffer, sizeof(dmar_uint_t) * group->bdshot.dma_tx_size);
#ifdef HAL_TIM_UP_SHARED
// although it should be possible to start the next DMAR transaction concurrently with receiving
// telemetry, in practice it seems to interfere with the DMA engine
if (group->shared_up_dma && group->bdshot.enabled) {
// next dshot pulse can go out now
chEvtSignalI(group->dshot_waiter, DSHOT_CASCADE);
}
#endif
// if using input capture DMA and sharing the UP and CH channels then clean up
// by assigning the source back to UP
#if STM32_DMA_SUPPORTS_DMAMUX
@ -508,6 +513,8 @@ __RAMFUNC__ void RCOutput::bdshot_finish_dshot_gcr_transaction(virtual_timer_t*
// we will not get data from active channels
group->bdshot.curr_telem_chan = bdshot_find_next_ic_channel(*group);
// dshot commands are issued without a response coming back, this allows
// us to handle the next packet correctly without it looking like a failure
if (group->bdshot.dma_tx_size > 0) {
group->dshot_state = DshotState::RECV_COMPLETE;
} else {
@ -738,7 +745,7 @@ bool RCOutput::bdshot_decode_telemetry_from_erpm(uint16_t encodederpm, uint8_t c
uint8_t normalized_chan = chan;
#endif
#if HAL_WITH_IO_MCU
if (AP_BoardConfig::io_dshot()) {
if (iomcu_dshot) {
normalized_chan = chan + chan_offset;
}
#endif

2
libraries/AP_HAL_ChibiOS/RCOutput_iofirmware.cpp
View File

@ -68,7 +68,7 @@ void RCOutput::dshot_send_trampoline(void *p)
*/
void RCOutput::rcout_thread() {
// don't start outputting until fully configured
while (!hal.scheduler->is_system_initialized() || _dshot_period_us == 0) {
while (!hal.scheduler->is_system_initialized()) {
hal.scheduler->delay_microseconds(1000);
}

2
libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp
View File

@ -100,7 +100,7 @@ void RCOutput::send_dshot_command(uint8_t command, uint8_t chan, uint32_t comman
// not an FMU channel
if (chan < chan_offset || chan == ALL_CHANNELS) {
#if HAL_WITH_IO_MCU
if (AP_BoardConfig::io_dshot()) {
if (iomcu_dshot) {
iomcu.send_dshot_command(command, chan, command_timeout_ms, repeat_count, priority);
}
#endif

3
libraries/AP_HAL_ChibiOS/hwdef/iomcu-f103-dshot/hwdef.dat
View File

@ -19,7 +19,7 @@ undef PA0 PA1 PB8 PB9
# the order is important here as it determines the order that timers are used to sending dshot
# TIM4 needs to go first so that TIM4_UP can be freed up to be used by input capture for TIM2
PB8 TIM4_CH3 TIM4 PWM(3) GPIO(103) BIDIR
PB8 TIM4_CH3 TIM4 PWM(3) GPIO(103) BIDIR UP_SHARED
PB9 TIM4_CH4 TIM4 PWM(4) GPIO(104)
PA0 TIM2_CH1 TIM2 PWM(1) GPIO(101)
PA1 TIM2_CH2 TIM2 PWM(2) GPIO(102) BIDIR # DMA channel 7, shared with TIM4_UP and USART2_TX
@ -43,7 +43,6 @@ define STM32_TIM_TIM4_CH3_DMA_CHAN 1
define STM32_TIM_TIM3_CH4_DMA_STREAM STM32_DMA_STREAM_ID(1, 3)
define STM32_TIM_TIM3_CH4_DMA_CHAN 1
define HAL_TIM4_UP_SHARED 1
undef SHARED_DMA_MASK
define SHARED_DMA_MASK (1U<<STM32_TIM_TIM4_UP_DMA_STREAM | 1U<<STM32_TIM_TIM2_CH2_DMA_STREAM | 1U<<STM32_UART_USART2_TX_DMA_STREAM)

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

@ -123,6 +123,9 @@ class ChibiOSHWDef(object):
# integer defines
self.intdefines = {}
# list of shared up timers
self.shared_up = []
def is_int(self, str):
'''check if a string is an integer'''
try:
@ -2067,6 +2070,7 @@ INCLUDE common.ld
rc_in_int = None
alarm = None
bidir = None
up_shared = None
pwm_out = []
# start with the ordered list from the dma resolver
pwm_timers = ordered_timers
@ -2084,6 +2088,8 @@ INCLUDE common.ld
pwm_out.append(p)
if p.has_extra('BIDIR'):
bidir = p
if p.has_extra('UP_SHARED'):
up_shared = p
if p.type not in pwm_timers:
pwm_timers.append(p.type)
@ -2170,6 +2176,10 @@ INCLUDE common.ld
f.write('// PWM timer config\n')
if bidir is not None:
f.write('#define HAL_WITH_BIDIR_DSHOT\n')
if up_shared is not None:
f.write('#define HAL_TIM_UP_SHARED\n')
for t in self.shared_up:
f.write('#define HAL_%s_SHARED true\n' % t)
for t in pwm_timers:
n = int(t[3:])
f.write('#define STM32_PWM_USE_TIM%u TRUE\n' % n)
@ -2225,7 +2235,10 @@ INCLUDE common.ld
# define HAL_IC%u_CH%u_DMA_CONFIG false, 0, 0
#endif
''' % (n, i, n, i, n, i, n, i, n, i, n, i)
if up_shared is not None:
hal_icu_cfg += '}, HAL_TIM%u_UP_SHARED, \\' % n
else:
hal_icu_cfg += '}, \\'
f.write('''#if defined(STM32_TIM_TIM%u_UP_DMA_STREAM) && defined(STM32_TIM_TIM%u_UP_DMA_CHAN)
# define HAL_PWM%u_DMA_CONFIG true, STM32_TIM_TIM%u_UP_DMA_STREAM, STM32_TIM_TIM%u_UP_DMA_CHAN
@ -2738,6 +2751,7 @@ INCLUDE common.ld
def build_peripheral_list(self):
'''build a list of peripherals for DMA resolver to work on'''
peripherals = []
self.shared_up = []
done = set()
prefixes = ['SPI', 'USART', 'UART', 'I2C']
periph_pins = self.allpins[:]
@ -2788,6 +2802,8 @@ INCLUDE common.ld
(_, _, compl) = self.parse_timer(ch_label)
if ch_label not in peripherals and p.has_extra('BIDIR') and not compl:
peripherals.append(ch_label)
if label not in self.shared_up and p.has_extra('UP_SHARED') and not compl:
self.shared_up.append(label)
done.add(type)
return peripherals

13
libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_iofirmware.h
View File

@ -41,3 +41,16 @@
#ifndef AP_INTERNALERROR_ENABLED
#define AP_INTERNALERROR_ENABLED 0
#endif
#ifndef HAL_TIM2_UP_SHARED
#define HAL_TIM2_UP_SHARED 0
#endif
#ifndef HAL_TIM3_UP_SHARED
#define HAL_TIM3_UP_SHARED 0
#endif
#ifndef HAL_TIM4_UP_SHARED
#define HAL_TIM4_UP_SHARED 0
#endif
#ifndef HAL_TIM_UP_SHARED
#define HAL_TIM_UP_SHARED (HAL_TIM2_UP_SHARED || HAL_TIM3_UP_SHARED || HAL_TIM4_UP_SHARED)
#endif