mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-05 15:38:29 -04:00
HAL_ChibiOS: fixed DMA disable on contention
this fixes a problem with the automatic DMA disable on DMA contention in UARTs. This fixes issue #14581 the problem was that while tx_dma_enabled was correctly set to false, it would keep looping inside write_pending_bytes_DMA() if the data arrived in the write buffer at a faster rate than it could be sent out, which did happen with a mavlink stream rate of 4Hz. This means it kept using DMA even with tx_dma_enabled set to false. The result was that the automatic flow control code never got a chance to run and we didn't switch back to non-DMA for these low baudrate contended UARTs
This commit is contained in:
parent
3128d71aac
commit
67fea9f2f8
@ -891,7 +891,7 @@ void UARTDriver::write_pending_bytes_DMA(uint32_t n)
|
||||
return; // all done
|
||||
}
|
||||
// find out how much is still left to write while we still have the lock
|
||||
n = _writebuf.available();
|
||||
n = MIN(_writebuf.available(), n);
|
||||
}
|
||||
|
||||
dma_handle->lock(); // we have our own thread so grab the lock
|
||||
@ -907,6 +907,8 @@ void UARTDriver::write_pending_bytes_DMA(uint32_t n)
|
||||
// low baudrate. Switch off DMA for future
|
||||
// transmits on this low baudrate UART
|
||||
tx_dma_enabled = false;
|
||||
dma_handle->unlock(false);
|
||||
break;
|
||||
}
|
||||
}
|
||||
/*
|
||||
@ -976,6 +978,13 @@ void UARTDriver::write_pending_bytes_DMA(uint32_t n)
|
||||
// update stats
|
||||
_total_written += tx_len;
|
||||
_tx_stats_bytes += tx_len;
|
||||
|
||||
n -= tx_len;
|
||||
} else {
|
||||
// if we didn't manage to transmit any bytes then stop
|
||||
// processing so we can check flow control state in outer
|
||||
// loop
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user