Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-09-21 23:44:48 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

refactor Fibre to AsyncStream architecture

Browse Source 
The previous implementation was based on blocking calls, i.e. the
platform dependent output channel would block on a write operation and
by extension the Fibre decoder could also block when given data by an
input channel. The revised implementation in this commit is based around
a fully asynchronous (i.e. non-blocking)  stream interface.
This commit is contained in:
Samuel Sadok
2020-09-15 12:57:39 +02:00
parent d2b469eca0
commit a1c67e6ba1
75 changed files with 5761 additions and 2078 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/communication/interface_uart.cpp
+143 -60
View File
@@ -6,9 +6,12 @@
#include <MotorControl/utils.hpp>
#include <fibre/protocol.hpp>
#include <fibre/../../async_stream.hpp>
#include <fibre/../../legacy_protocol.hpp>
#include <usart.h>
#include <cmsis_os.h>
#include <freertos_vars.h>
#include <odrive_main.h>
#define UART_TX_BUFFER_SIZE 64
#define UART_RX_BUFFER_SIZE 64
@@ -26,78 +29,156 @@ extern UART_HandleTypeDef* uart0;
static UART_HandleTypeDef* huart_ = uart0; // defined in board.cpp.
const uint32_t stack_size_uart_thread = 4096; // Bytes
namespace fibre {
class UARTSender : public StreamSink {
class Stm32UartTxStream : public AsyncStreamSink {
public:
int process_bytes(const uint8_t* buffer, size_t length, size_t* processed_bytes) {
// Loop to ensure all bytes get sent
while (length) {
size_t chunk = length < UART_TX_BUFFER_SIZE ? length : UART_TX_BUFFER_SIZE;
// wait for USB interface to become ready
// TODO: implement ring buffer to get a more continuous stream of data
// if (osSemaphoreWait(sem_uart_dma, deadline_to_timeout(deadline_ms)) != osOK)
if (osSemaphoreWait(sem_uart_dma, PROTOCOL_SERVER_TIMEOUT_MS) != osOK)
return -1;
// transmit chunk
memcpy(tx_buf_, buffer, chunk);
if (HAL_UART_Transmit_DMA(huart_, tx_buf_, chunk) != HAL_OK)
return -1;
buffer += chunk;
length -= chunk;
if (processed_bytes)
*processed_bytes += chunk;
}
return 0;
Stm32UartTxStream(UART_HandleTypeDef* huart) : huart_(huart) {}
void start_write(cbufptr_t buffer, TransferHandle* handle, Completer<WriteResult>& completer) final;
void cancel_write(TransferHandle transfer_handle) final;
void did_finish();
UART_HandleTypeDef *huart_;
Completer<WriteResult>* completer_ = nullptr;
const uint8_t* tx_end_ = nullptr;
};
class Stm32UartRxStream : public AsyncStreamSource {
public:
void start_read(bufptr_t buffer, TransferHandle* handle, Completer<ReadResult>& completer) final;
void cancel_read(TransferHandle transfer_handle) final;
void did_receive(uint8_t* buffer, size_t length);
Completer<ReadResult>* completer_ = nullptr;
bufptr_t rx_buf_ = {nullptr, nullptr};
};
}
using namespace fibre;
void Stm32UartTxStream::start_write(cbufptr_t buffer, TransferHandle* handle, Completer<WriteResult>& completer) {
size_t chunk = std::min(buffer.size(), (size_t)UART_TX_BUFFER_SIZE);
completer_ = &completer;
tx_end_ = buffer.begin() + chunk;
if (handle) {
*handle = reinterpret_cast<TransferHandle>(this);
}
size_t get_free_space() { return SIZE_MAX; }
private:
uint8_t tx_buf_[UART_TX_BUFFER_SIZE];
} uart_stream_output;
StreamSink* uart_stream_output_ptr = &uart_stream_output;
if (HAL_UART_Transmit_DMA(huart_, const_cast<uint8_t*>(buffer.begin()), chunk) != HAL_OK) {
completer_ = nullptr;
tx_end_ = nullptr;
completer.complete({kStreamError, buffer.begin()});
}
}
StreamBasedPacketSink uart_packet_output(uart_stream_output);
BidirectionalPacketBasedChannel uart_channel(uart_packet_output);
StreamToPacketSegmenter uart_stream_input(uart_channel);
void Stm32UartTxStream::cancel_write(TransferHandle transfer_handle) {
// not implemented
}
void Stm32UartTxStream::did_finish() {
const uint8_t* tx_end = tx_end_;
tx_end_ = nullptr;
safe_complete(completer_, {kStreamOk, tx_end});
}
void Stm32UartRxStream::start_read(bufptr_t buffer, TransferHandle* handle, Completer<ReadResult>& completer) {
completer_ = &completer;
rx_buf_ = buffer;
if (handle) {
*handle = reinterpret_cast<TransferHandle>(this);
}
}
void Stm32UartRxStream::cancel_read(TransferHandle transfer_handle) {
// not implemented
}
void Stm32UartRxStream::did_receive(uint8_t* buffer, size_t length) {
// This can be called even if there was no RX operation in progress
bufptr_t rx_buf = rx_buf_;
if (completer_ && rx_buf.begin()) {
rx_buf_ = {nullptr, nullptr};
size_t chunk = std::min(length, rx_buf.size());
memcpy(rx_buf.begin(), buffer, chunk);
safe_complete(completer_, {kStreamOk, rx_buf.begin() + chunk});
}
}
Stm32UartTxStream uart_tx_stream(huart_);
Stm32UartRxStream uart_rx_stream;
LegacyProtocolStreamBased fibre_over_uart(&uart_rx_stream, &uart_tx_stream);
fibre::AsyncStreamSinkMultiplexer<2> uart_tx_multiplexer(uart_tx_stream);
fibre::BufferedStreamSink<64> uart0_stdout_sink(uart_tx_multiplexer); // Used in communication.cpp
AsciiProtocol ascii_over_uart(&uart_rx_stream, &uart_tx_multiplexer);
bool uart0_stdout_pending = false;
static void uart_server_thread(void * ctx) {
(void) ctx;
if (odrv.config_.uart0_protocol == ODrive::STREAM_PROTOCOL_TYPE_FIBRE) {
fibre_over_uart.start(Completer<LegacyProtocolPacketBased*, StreamStatus>::get_dummy());
} else if (odrv.config_.uart0_protocol == ODrive::STREAM_PROTOCOL_TYPE_ASCII
|| odrv.config_.uart0_protocol == ODrive::STREAM_PROTOCOL_TYPE_ASCII_AND_STDOUT) {
ascii_over_uart.start();
}
for (;;) {
// Check for UART errors and restart receive DMA transfer if required
if (huart_->RxState != HAL_UART_STATE_BUSY_RX) {
HAL_UART_AbortReceive(huart_);
HAL_UART_Receive_DMA(huart_, dma_rx_buffer, sizeof(dma_rx_buffer));
dma_last_rcv_idx = 0;
}
// Fetch the circular buffer "write pointer", where it would write next
uint32_t new_rcv_idx = UART_RX_BUFFER_SIZE - huart_->hdmarx->Instance->NDTR;
if (new_rcv_idx > UART_RX_BUFFER_SIZE) { // defensive programming
osEvent event = osMessageGet(uart_event_queue, osWaitForever);
if (event.status != osEventMessage) {
continue;
}
// deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS);
// Process bytes in one or two chunks (two in case there was a wrap)
if (new_rcv_idx < dma_last_rcv_idx) {
uart_stream_input.process_bytes(dma_rx_buffer + dma_last_rcv_idx,
UART_RX_BUFFER_SIZE - dma_last_rcv_idx, nullptr); // TODO: use process_all
ASCII_protocol_parse_stream(dma_rx_buffer + dma_last_rcv_idx,
UART_RX_BUFFER_SIZE - dma_last_rcv_idx, uart_stream_output);
dma_last_rcv_idx = 0;
}
if (new_rcv_idx > dma_last_rcv_idx) {
uart_stream_input.process_bytes(dma_rx_buffer + dma_last_rcv_idx,
new_rcv_idx - dma_last_rcv_idx, nullptr); // TODO: use process_all
ASCII_protocol_parse_stream(dma_rx_buffer + dma_last_rcv_idx,
new_rcv_idx - dma_last_rcv_idx, uart_stream_output);
dma_last_rcv_idx = new_rcv_idx;
}
switch (event.value.v) {
case 1: {
// This event is triggered by the control loop at 8kHz. This should be
// enough for most applications.
// At 1Mbaud/s that corresponds to at most 12.5 bytes which can arrive
// during the sleep period.
// The thread is woken up by the control loop at 8kHz. This should be
// enough for most applications.
// At 1Mbaud/s that corresponds to at most 12.5 bytes which can arrive
// during the sleep period.
osThreadSuspend(nullptr);
// Check for UART errors and restart receive DMA transfer if required
if (huart_->RxState != HAL_UART_STATE_BUSY_RX) {
HAL_UART_AbortReceive(&huart4);
HAL_UART_Receive_DMA(&huart4, dma_rx_buffer, sizeof(dma_rx_buffer));
dma_last_rcv_idx = 0;
}
// Fetch the circular buffer "write pointer", where it would write next
uint32_t new_rcv_idx = UART_RX_BUFFER_SIZE - huart_->hdmarx->Instance->NDTR;
if (new_rcv_idx > UART_RX_BUFFER_SIZE) { // defensive programming
continue;
}
// Process bytes in one or two chunks (two in case there was a wrap)
if (new_rcv_idx < dma_last_rcv_idx) {
uart_rx_stream.did_receive(dma_rx_buffer + dma_last_rcv_idx,
UART_RX_BUFFER_SIZE - dma_last_rcv_idx);
dma_last_rcv_idx = 0;
}
if (new_rcv_idx > dma_last_rcv_idx) {
uart_rx_stream.did_receive(dma_rx_buffer + dma_last_rcv_idx,
new_rcv_idx - dma_last_rcv_idx);
dma_last_rcv_idx = new_rcv_idx;
}
} break;
case 2: {
uart_tx_stream.did_finish();
} break;
case 3: { // stdout has data
uart0_stdout_pending = false;
uart0_stdout_sink.maybe_start_async_write();
} break;
}
}
}
@@ -116,10 +197,12 @@ void start_uart_server() {
void uart_poll() {
if (uart_thread) { // the thread is only started if UART is enabled
osThreadResume(uart_thread);
osMessagePut(uart_event_queue, 1, 0);
}
}
void HAL_UART_TxCpltCallback(UART_HandleTypeDef* huart) {
osSemaphoreRelease(sem_uart_dma);
if (huart == &huart4) {
osMessagePut(uart_event_queue, 2, 0);
}
}