diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index 1817c3e681..4c995188b6 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -1535,16 +1535,15 @@ void RCOutput::dma_deallocate(Shared_DMA *ctx) for (auto &group : pwm_group_list) { if (group.dma_handle == ctx && group.dma != nullptr) { chSysLock(); + dmaStreamFreeI(group.dma); #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 // that we don't have to worry about line modes etc if (group.pwm_started && group.dma_handle->is_shared()) { - group.pwm_drv->tim->CR1 = 0; - group.pwm_drv->tim->DIER = 0; + bdshot_disable_pwm_f1(group); } #endif - dmaStreamFreeI(group.dma); group.dma = nullptr; chSysUnlock(); } diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.h b/libraries/AP_HAL_ChibiOS/RCOutput.h index 8fbfa6615b..0bf7bcd1c6 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.h +++ b/libraries/AP_HAL_ChibiOS/RCOutput.h @@ -725,6 +725,7 @@ private: static void bdshot_receive_pulses_DMAR_f1(pwm_group* group); static void bdshot_reset_pwm(pwm_group& group, uint8_t telem_channel); static void bdshot_reset_pwm_f1(pwm_group& group, uint8_t telem_channel); + static void bdshot_disable_pwm_f1(pwm_group& group); static void bdshot_config_icu_dshot(stm32_tim_t* TIMx, uint8_t chan, uint8_t ccr_ch); static void bdshot_config_icu_dshot_f1(stm32_tim_t* TIMx, uint8_t chan, uint8_t ccr_ch); static uint32_t bdshot_get_output_rate_hz(const enum output_mode mode); diff --git a/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp b/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp index 7d4002cc23..d0badaa021 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp @@ -230,6 +230,11 @@ void RCOutput::bdshot_ic_dma_deallocate(Shared_DMA *ctx) chSysLock(); if (group.bdshot.ic_dma_handle[icuch] == ctx && group.bdshot.ic_dma[icuch] != nullptr) { dmaStreamFreeI(group.bdshot.ic_dma[icuch]); +#if defined(STM32F1) + if (group.bdshot.ic_dma_handle[icuch]->is_shared()) { + bdshot_disable_pwm_f1(group); + } +#endif group.bdshot.ic_dma[icuch] = nullptr; } chSysUnlock(); diff --git a/libraries/AP_HAL_ChibiOS/RCOutput_iofirmware.cpp b/libraries/AP_HAL_ChibiOS/RCOutput_iofirmware.cpp index c8d535dc5b..645e77906c 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput_iofirmware.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput_iofirmware.cpp @@ -134,6 +134,22 @@ void RCOutput::rcout_thread() { } } +#if defined(STM32F1) +void RCOutput::bdshot_disable_pwm_f1(pwm_group& group) +{ + stm32_tim_t* TIMx = group.pwm_drv->tim; + // pwmStop sets these + TIMx->CR1 = 0; /* Timer disabled. */ + TIMx->DIER = 0; /* All IRQs disabled. */ + TIMx->SR = 0; /* Clear eventual pending IRQs. */ + TIMx->CNT = 0; + TIMx->CCR[0] = 0; /* Comparator 1 disabled. */ + TIMx->CCR[1] = 0; /* Comparator 2 disabled. */ + TIMx->CCR[2] = 0; /* Comparator 3 disabled. */ + TIMx->CCR[3] = 0; /* Comparator 4 disabled. */ +} +#endif + #if defined(HAL_WITH_BIDIR_DSHOT) && defined(STM32F1) // reset pwm driver to output mode without resetting the clock or the peripheral // the code here is the equivalent of pwmStart()/pwmStop() @@ -142,15 +158,8 @@ void RCOutput::bdshot_reset_pwm_f1(pwm_group& group, uint8_t telem_channel) osalSysLock(); stm32_tim_t* TIMx = group.pwm_drv->tim; - // pwmStop sets these - TIMx->CR1 = 0; /* Timer disabled. */ - TIMx->DIER = 0; /* All IRQs disabled. */ - TIMx->SR = 0; /* Clear eventual pending IRQs. */ - TIMx->CNT = 0; - TIMx->CCR[0] = 0; /* Comparator 1 disabled. */ - TIMx->CCR[1] = 0; /* Comparator 2 disabled. */ - TIMx->CCR[2] = 0; /* Comparator 3 disabled. */ - TIMx->CCR[3] = 0; /* Comparator 4 disabled. */ + bdshot_disable_pwm_f1(group); + // at the point this is called we will have done input capture on two CC channels // we need to switch those channels back to output and the default settings // all other channels will not have been modified