diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index e0539b01dd..d8c5a218a2 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -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<is_shared()) { @@ -1516,9 +1520,9 @@ void RCOutput::dma_allocate(Shared_DMA *ctx) dmaSetRequestSource(group.dma, group.dma_up_channel); } #endif + chSysUnlock(); } } - 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(); } } - 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 diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.h b/libraries/AP_HAL_ChibiOS/RCOutput.h index d6c6c6a343..0b0a41b811 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.h +++ b/libraries/AP_HAL_ChibiOS/RCOutput.h @@ -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); diff --git a/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp b/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp index 8817e0217e..43664570eb 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp @@ -54,7 +54,7 @@ void RCOutput::set_bidir_dshot_mask(uint32_t mask) { #if HAL_WITH_IO_MCU const uint32_t iomcu_mask = ((1U<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 diff --git a/libraries/AP_HAL_ChibiOS/RCOutput_iofirmware.cpp b/libraries/AP_HAL_ChibiOS/RCOutput_iofirmware.cpp index e45ce3c363..994485e4e9 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput_iofirmware.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput_iofirmware.cpp @@ -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); } diff --git a/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp b/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp index 52c801e985..c8ca5156fb 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp @@ -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 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/iomcu-f103-dshot/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/iomcu-f103-dshot/hwdef.dat index 9f885359fe..10b71b35a9 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/iomcu-f103-dshot/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/iomcu-f103-dshot/hwdef.dat @@ -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<