diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index d1f4366e994b22..365944433494fc 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 @@ -1196,7 +1200,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; } @@ -1232,7 +1236,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_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 @@ -509,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 { @@ -739,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_serial.cpp b/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp index 52c801e985f7c8..c8ca5156fb7ce2 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 9f885359fe2ddb..10b71b35a9a0ad 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<