Skip to content
18 changes: 18 additions & 0 deletions src/main/config/config_eeprom.c
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,7 @@

#include "drivers/system.h"
#include "drivers/flash.h"
#include "drivers/pwm_output.h"

#include "fc/config.h"

Expand Down Expand Up @@ -321,6 +322,18 @@ static bool writeSettingsToEEPROM(void)

void writeConfigToEEPROM(void)
{
#if !defined(SITL_BUILD) && defined(USE_DSHOT)
// Prevent ESC spinup during settings save using circular DMA
pwmSetMotorDMACircular(true);

// Force motor updates to latch current (zero) throttle into circular DMA buffer
pwmCompleteMotorUpdate();
delayMicroseconds(200);
pwmCompleteMotorUpdate();
delayMicroseconds(200);
pwmCompleteMotorUpdate();
#endif

bool success = false;
// write it
for (int attempt = 0; attempt < 3 && !success; attempt++) {
Expand All @@ -333,6 +346,11 @@ void writeConfigToEEPROM(void)
}
}

#if !defined(SITL_BUILD) && defined(USE_DSHOT)
// Restore normal DMA mode
pwmSetMotorDMACircular(false);
#endif

if (success && isEEPROMContentValid()) {
return;
}
Expand Down
14 changes: 14 additions & 0 deletions src/main/drivers/pwm_output.c
Original file line number Diff line number Diff line change
Expand Up @@ -226,6 +226,20 @@ void pwmEnableMotors(void)
pwmMotorsEnabled = true;
}

void pwmSetMotorDMACircular(bool circular)
{
#ifdef USE_DSHOT
// Set DMA circular mode for all motor outputs
for (int i = 0; i < getMotorCount(); i++) {
if (motors[i].pwmPort && motors[i].pwmPort->tch) {
impl_timerPWMSetDMACircular(motors[i].pwmPort->tch, circular);
}
}
#else
UNUSED(circular);
#endif
}

bool isMotorBrushed(uint16_t motorPwmRateHz)
{
return (motorPwmRateHz > 500);
Expand Down
1 change: 1 addition & 0 deletions src/main/drivers/pwm_output.h
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,7 @@ void pwmWriteServo(uint8_t index, uint16_t value);

void pwmDisableMotors(void);
void pwmEnableMotors(void);
void pwmSetMotorDMACircular(bool circular);
struct timerHardware_s;

void pwmMotorPreconfigure(void);
Expand Down
1 change: 1 addition & 0 deletions src/main/drivers/timer_impl.h
Original file line number Diff line number Diff line change
Expand Up @@ -84,6 +84,7 @@ bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint8_t dmaBuf
void impl_timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferElementCount);
void impl_timerPWMStartDMA(TCH_t * tch);
void impl_timerPWMStopDMA(TCH_t * tch);
void impl_timerPWMSetDMACircular(TCH_t * tch, bool circular);

#ifdef USE_DSHOT_DMAR
bool impl_timerPWMConfigDMABurst(burstDmaTimer_t *burstDmaTimer, TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount);
Expand Down
58 changes: 58 additions & 0 deletions src/main/drivers/timer_impl_hal.c
Original file line number Diff line number Diff line change
Expand Up @@ -580,3 +580,61 @@ void impl_timerPWMStopDMA(TCH_t * tch)

HAL_TIM_Base_Start(tch->timCtx->timHandle);
}

void impl_timerPWMSetDMACircular(TCH_t * tch, bool circular)
{
if (!tch->dma || !tch->dma->dma) {
return;
}

const uint32_t streamLL = lookupDMALLStreamTable[DMATAG_GET_STREAM(tch->timHw->dmaTag)];
DMA_TypeDef *dmaBase = tch->dma->dma;

// Save the current transfer count before disabling
uint32_t dataLength = LL_DMA_GetDataLength(dmaBase, streamLL);

// Protect DMA reconfiguration from interrupt interference
ATOMIC_BLOCK(NVIC_PRIO_MAX) {
// Disable timer DMA request first to stop new transfer triggers
LL_TIM_DisableDMAReq_CCx(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex]);

// Disable the DMA stream
LL_DMA_DisableStream(dmaBase, streamLL);

// CRITICAL: Wait for stream to actually become disabled
// The EN bit doesn't clear immediately, especially if transfer is in progress
uint32_t timeout = 10000;
while (LL_DMA_IsEnabledStream(dmaBase, streamLL) && timeout--) {
__NOP();
}

// If timeout occurred, DMA stream is still enabled - abort reconfiguration
if (timeout == 0 && LL_DMA_IsEnabledStream(dmaBase, streamLL)) {
// Re-enable timer DMA request and return to avoid unstable state
LL_TIM_EnableDMAReq_CCx(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex]);
return;
}

// Clear any pending transfer complete flags
DMA_CLEAR_FLAG(tch->dma, DMA_IT_TCIF);

// Modify the DMA mode
if (circular) {
LL_DMA_SetMode(dmaBase, streamLL, LL_DMA_MODE_CIRCULAR);
} else {
LL_DMA_SetMode(dmaBase, streamLL, LL_DMA_MODE_NORMAL);
}

// Reload the transfer count (required after mode change)
// If dataLength was 0 (transfer completed), keep it at 0 - the next motor update will reload it
if (dataLength > 0) {
LL_DMA_SetDataLength(dmaBase, streamLL, dataLength);
}

// Re-enable DMA stream
LL_DMA_EnableStream(dmaBase, streamLL);

// Re-enable timer DMA requests
LL_TIM_EnableDMAReq_CCx(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex]);
}
}
36 changes: 36 additions & 0 deletions src/main/drivers/timer_impl_stdperiph.c
Original file line number Diff line number Diff line change
Expand Up @@ -519,3 +519,39 @@ void impl_timerPWMStopDMA(TCH_t * tch)
tch->dmaState = TCH_DMA_IDLE;
TIM_Cmd(tch->timHw->tim, ENABLE);
}

void impl_timerPWMSetDMACircular(TCH_t * tch, bool circular)
{
if (!tch->dma || !tch->dma->ref) {
return;
}

// Protect DMA reconfiguration from interrupt interference
ATOMIC_BLOCK(NVIC_PRIO_MAX) {
// Temporarily disable DMA while modifying configuration
DMA_Cmd(tch->dma->ref, DISABLE);

// Wait for DMA stream to actually be disabled
// The EN bit doesn't clear immediately, especially if transfer is in progress
uint32_t timeout = 10000;
while ((tch->dma->ref->CR & DMA_SxCR_EN) && timeout--) {
__NOP();
}

// If timeout occurred, DMA stream is still enabled - abort reconfiguration
if (timeout == 0 && (tch->dma->ref->CR & DMA_SxCR_EN)) {
DMA_Cmd(tch->dma->ref, ENABLE); // Re-enable and return
return;
}

// Modify the DMA mode
if (circular) {
tch->dma->ref->CR |= DMA_SxCR_CIRC; // Set circular bit
} else {
tch->dma->ref->CR &= ~DMA_SxCR_CIRC; // Clear circular bit
}

// Re-enable DMA
DMA_Cmd(tch->dma->ref, ENABLE);
}
}
35 changes: 35 additions & 0 deletions src/main/drivers/timer_impl_stdperiph_at32.c
Original file line number Diff line number Diff line change
Expand Up @@ -407,3 +407,38 @@ void impl_timerPWMStopDMA(TCH_t * tch)
tch->dmaState = TCH_DMA_IDLE;
tmr_counter_enable(tch->timHw->tim, TRUE);
}

void impl_timerPWMSetDMACircular(TCH_t * tch, bool circular)
{
if (!tch->dma || !tch->dma->ref) {
return;
}

// Protect DMA reconfiguration from interrupt interference
ATOMIC_BLOCK(NVIC_PRIO_MAX) {
// Temporarily disable DMA while modifying configuration
dma_channel_enable(tch->dma->ref, FALSE);

// Wait for DMA channel to actually be disabled
// The enable bit doesn't clear immediately, especially if transfer is in progress
uint32_t timeout = 10000;
while (tch->dma->ref->ctrl_bit.chen && timeout--) {
__NOP();
}

// If timeout occurred, DMA channel is still enabled - abort reconfiguration
if (timeout == 0 && tch->dma->ref->ctrl_bit.chen) {
dma_channel_enable(tch->dma->ref, TRUE); // Re-enable and return
return;
}

if (circular) {
tch->dma->ref->ctrl_bit.lm = TRUE; // Enable loop mode
} else {
tch->dma->ref->ctrl_bit.lm = FALSE; // Disable loop mode
}

// Re-enable DMA
dma_channel_enable(tch->dma->ref, TRUE);
}
}