mirror of https://github.com/commaai/panda.git
UART instability fix with high interrupt load (#283)
* Fixed UART overrun error * Added stability test scripts * Refactored UART code. ESP/GPS now uses DMA in circular mode to directly write into the ring buffer, saving a bunch of interrupts and potential race conditions. * Changed stability test to use amount of bytes instead of amount of non-zero messages, since the ring buffer pointer is only updated on half or full DMA transfer or on line idle (e.g. no more 1 char messages from the gps) * Increase data limit. It's too low sometimes in normal operation * Forgot to set write pointer in ring buffer * Increased test limit even more
This commit is contained in:
robbederks
GitHub
parent
9a9e9d47bc
commit
9486836886
|
|
@ -2,6 +2,7 @@
|
||||||
#define PANDA_CONFIG_H
|
#define PANDA_CONFIG_H
|
||||||
|
|
||||||
//#define DEBUG
|
//#define DEBUG
|
||||||
|
//#define DEBUG_UART
|
||||||
//#define DEBUG_USB
|
//#define DEBUG_USB
|
||||||
//#define DEBUG_SPI
|
//#define DEBUG_SPI
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -1,18 +1,43 @@
|
||||||
// IRQs: USART1, USART2, USART3, UART5
|
// IRQs: USART1, USART2, USART3, UART5
|
||||||
|
|
||||||
#define FIFO_SIZE 0x400U
|
// ***************************** Definitions *****************************
|
||||||
|
#define FIFO_SIZE_INT 0x400U
|
||||||
|
#define FIFO_SIZE_DMA 0x1000U
|
||||||
|
|
||||||
typedef struct uart_ring {
|
typedef struct uart_ring {
|
||||||
volatile uint16_t w_ptr_tx;
|
volatile uint16_t w_ptr_tx;
|
||||||
volatile uint16_t r_ptr_tx;
|
volatile uint16_t r_ptr_tx;
|
||||||
uint8_t elems_tx[FIFO_SIZE];
|
uint8_t *elems_tx;
|
||||||
|
uint32_t tx_fifo_size;
|
||||||
volatile uint16_t w_ptr_rx;
|
volatile uint16_t w_ptr_rx;
|
||||||
volatile uint16_t r_ptr_rx;
|
volatile uint16_t r_ptr_rx;
|
||||||
uint8_t elems_rx[FIFO_SIZE];
|
uint8_t *elems_rx;
|
||||||
|
uint32_t rx_fifo_size;
|
||||||
USART_TypeDef *uart;
|
USART_TypeDef *uart;
|
||||||
void (*callback)(struct uart_ring*);
|
void (*callback)(struct uart_ring*);
|
||||||
|
bool dma_rx;
|
||||||
} uart_ring;
|
} uart_ring;
|
||||||
|
|
||||||
void uart_init(USART_TypeDef *u, int baud);
|
#define UART_BUFFER(x, size_rx, size_tx, uart_ptr, callback_ptr, rx_dma) \
|
||||||
|
uint8_t elems_rx_##x[size_rx]; \
|
||||||
|
uint8_t elems_tx_##x[size_tx]; \
|
||||||
|
uart_ring uart_ring_##x = { \
|
||||||
|
.w_ptr_tx = 0, \
|
||||||
|
.r_ptr_tx = 0, \
|
||||||
|
.elems_tx = ((uint8_t *)&elems_tx_##x), \
|
||||||
|
.tx_fifo_size = size_tx, \
|
||||||
|
.w_ptr_rx = 0, \
|
||||||
|
.r_ptr_rx = 0, \
|
||||||
|
.elems_rx = ((uint8_t *)&elems_rx_##x), \
|
||||||
|
.rx_fifo_size = size_rx, \
|
||||||
|
.uart = uart_ptr, \
|
||||||
|
.callback = callback_ptr, \
|
||||||
|
.dma_rx = rx_dma \
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
// ***************************** Function prototypes *****************************
|
||||||
|
void uart_init(uart_ring *q, int baud);
|
||||||
|
|
||||||
bool getc(uart_ring *q, char *elem);
|
bool getc(uart_ring *q, char *elem);
|
||||||
bool putc(uart_ring *q, char elem);
|
bool putc(uart_ring *q, char elem);
|
||||||
|
|
@ -21,48 +46,35 @@ void puts(const char *a);
|
||||||
void puth(unsigned int i);
|
void puth(unsigned int i);
|
||||||
void hexdump(const void *a, int l);
|
void hexdump(const void *a, int l);
|
||||||
|
|
||||||
|
void debug_ring_callback(uart_ring *ring);
|
||||||
|
|
||||||
// ***************************** serial port queues *****************************
|
// ******************************** UART buffers ********************************
|
||||||
|
|
||||||
// esp = USART1
|
// esp_gps = USART1
|
||||||
uart_ring esp_ring = { .w_ptr_tx = 0, .r_ptr_tx = 0,
|
UART_BUFFER(esp_gps, FIFO_SIZE_DMA, FIFO_SIZE_INT, USART1, NULL, true)
|
||||||
.w_ptr_rx = 0, .r_ptr_rx = 0,
|
|
||||||
.uart = USART1,
|
|
||||||
.callback = NULL};
|
|
||||||
|
|
||||||
// lin1, K-LINE = UART5
|
// lin1, K-LINE = UART5
|
||||||
// lin2, L-LINE = USART3
|
// lin2, L-LINE = USART3
|
||||||
uart_ring lin1_ring = { .w_ptr_tx = 0, .r_ptr_tx = 0,
|
UART_BUFFER(lin1, FIFO_SIZE_INT, FIFO_SIZE_INT, UART5, NULL, false)
|
||||||
.w_ptr_rx = 0, .r_ptr_rx = 0,
|
UART_BUFFER(lin2, FIFO_SIZE_INT, FIFO_SIZE_INT, USART3, NULL, false)
|
||||||
.uart = UART5,
|
|
||||||
.callback = NULL};
|
|
||||||
uart_ring lin2_ring = { .w_ptr_tx = 0, .r_ptr_tx = 0,
|
|
||||||
.w_ptr_rx = 0, .r_ptr_rx = 0,
|
|
||||||
.uart = USART3,
|
|
||||||
.callback = NULL};
|
|
||||||
|
|
||||||
// debug = USART2
|
// debug = USART2
|
||||||
void debug_ring_callback(uart_ring *ring);
|
UART_BUFFER(debug, FIFO_SIZE_INT, FIFO_SIZE_INT, USART2, debug_ring_callback, false)
|
||||||
uart_ring debug_ring = { .w_ptr_tx = 0, .r_ptr_tx = 0,
|
|
||||||
.w_ptr_rx = 0, .r_ptr_rx = 0,
|
|
||||||
.uart = USART2,
|
|
||||||
.callback = debug_ring_callback};
|
|
||||||
|
|
||||||
|
|
||||||
uart_ring *get_ring_by_number(int a) {
|
uart_ring *get_ring_by_number(int a) {
|
||||||
uart_ring *ring = NULL;
|
uart_ring *ring = NULL;
|
||||||
switch(a) {
|
switch(a) {
|
||||||
case 0:
|
case 0:
|
||||||
ring = &debug_ring;
|
ring = &uart_ring_debug;
|
||||||
break;
|
break;
|
||||||
case 1:
|
case 1:
|
||||||
ring = &esp_ring;
|
ring = &uart_ring_esp_gps;
|
||||||
break;
|
break;
|
||||||
case 2:
|
case 2:
|
||||||
ring = &lin1_ring;
|
ring = &uart_ring_lin1;
|
||||||
break;
|
break;
|
||||||
case 3:
|
case 3:
|
||||||
ring = &lin2_ring;
|
ring = &uart_ring_lin2;
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
ring = NULL;
|
ring = NULL;
|
||||||
|
|
@ -71,52 +83,219 @@ uart_ring *get_ring_by_number(int a) {
|
||||||
return ring;
|
return ring;
|
||||||
}
|
}
|
||||||
|
|
||||||
// ***************************** serial port *****************************
|
// ***************************** Interrupt handlers *****************************
|
||||||
|
|
||||||
void uart_ring_process(uart_ring *q) {
|
void uart_tx_ring(uart_ring *q){
|
||||||
ENTER_CRITICAL();
|
ENTER_CRITICAL();
|
||||||
// TODO: check if external serial is connected
|
// Send out next byte of TX buffer
|
||||||
int sr = q->uart->SR;
|
|
||||||
|
|
||||||
if (q->w_ptr_tx != q->r_ptr_tx) {
|
if (q->w_ptr_tx != q->r_ptr_tx) {
|
||||||
if ((sr & USART_SR_TXE) != 0) {
|
// Only send if transmit register is empty (aka last byte has been sent)
|
||||||
q->uart->DR = q->elems_tx[q->r_ptr_tx];
|
if ((q->uart->SR & USART_SR_TXE) != 0) {
|
||||||
q->r_ptr_tx = (q->r_ptr_tx + 1U) % FIFO_SIZE;
|
q->uart->DR = q->elems_tx[q->r_ptr_tx]; // This clears TXE
|
||||||
|
q->r_ptr_tx = (q->r_ptr_tx + 1U) % q->tx_fifo_size;
|
||||||
}
|
}
|
||||||
// there could be more to send
|
|
||||||
q->uart->CR1 |= USART_CR1_TXEIE;
|
|
||||||
} else {
|
|
||||||
// nothing to send
|
|
||||||
q->uart->CR1 &= ~USART_CR1_TXEIE;
|
|
||||||
}
|
|
||||||
|
|
||||||
if ((sr & USART_SR_RXNE) || (sr & USART_SR_ORE)) {
|
// Enable TXE interrupt if there is still data to be sent
|
||||||
uint8_t c = q->uart->DR; // TODO: can drop packets
|
if(q->r_ptr_tx != q->w_ptr_tx){
|
||||||
if (q != &esp_ring) {
|
q->uart->CR1 |= USART_CR1_TXEIE;
|
||||||
uint16_t next_w_ptr = (q->w_ptr_rx + 1U) % FIFO_SIZE;
|
} else {
|
||||||
if (next_w_ptr != q->r_ptr_rx) {
|
q->uart->CR1 &= ~USART_CR1_TXEIE;
|
||||||
q->elems_rx[q->w_ptr_rx] = c;
|
}
|
||||||
q->w_ptr_rx = next_w_ptr;
|
}
|
||||||
if (q->callback != NULL) {
|
EXIT_CRITICAL();
|
||||||
q->callback(q);
|
}
|
||||||
}
|
|
||||||
|
void uart_rx_ring(uart_ring *q){
|
||||||
|
// Do not read out directly if DMA enabled
|
||||||
|
if (q->dma_rx == false) {
|
||||||
|
ENTER_CRITICAL();
|
||||||
|
|
||||||
|
// Read out RX buffer
|
||||||
|
uint8_t c = q->uart->DR; // This read after reading SR clears a bunch of interrupts
|
||||||
|
|
||||||
|
uint16_t next_w_ptr = (q->w_ptr_rx + 1U) % q->rx_fifo_size;
|
||||||
|
// Do not overwrite buffer data
|
||||||
|
if (next_w_ptr != q->r_ptr_rx) {
|
||||||
|
q->elems_rx[q->w_ptr_rx] = c;
|
||||||
|
q->w_ptr_rx = next_w_ptr;
|
||||||
|
if (q->callback != NULL) {
|
||||||
|
q->callback(q);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
EXIT_CRITICAL();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// This function should be called on:
|
||||||
|
// * Half-transfer DMA interrupt
|
||||||
|
// * Full-transfer DMA interrupt
|
||||||
|
// * UART IDLE detection
|
||||||
|
uint32_t prev_w_index = 0;
|
||||||
|
void dma_pointer_handler(uart_ring *q, uint32_t dma_ndtr) {
|
||||||
|
ENTER_CRITICAL();
|
||||||
|
uint32_t w_index = (q->rx_fifo_size - dma_ndtr);
|
||||||
|
|
||||||
|
// Check for new data
|
||||||
|
if (w_index != prev_w_index){
|
||||||
|
// Check for overflow
|
||||||
|
if (
|
||||||
|
((prev_w_index < q->r_ptr_rx) && (q->r_ptr_rx <= w_index)) || // No rollover
|
||||||
|
((w_index < prev_w_index) && ((q->r_ptr_rx <= w_index) || (prev_w_index < q->r_ptr_rx))) // Rollover
|
||||||
|
){
|
||||||
|
// We lost data. Set the new read pointer to the oldest byte still available
|
||||||
|
q->r_ptr_rx = (w_index + 1U) % q->rx_fifo_size;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Set write pointer
|
||||||
|
q->w_ptr_rx = w_index;
|
||||||
}
|
}
|
||||||
|
|
||||||
if ((sr & USART_SR_ORE) != 0) {
|
prev_w_index = w_index;
|
||||||
// set dropped packet flag?
|
EXIT_CRITICAL();
|
||||||
|
}
|
||||||
|
|
||||||
|
// This read after reading SR clears all error interrupts. We don't want compiler warnings, nor optimizations
|
||||||
|
#define UART_READ_DR(uart) volatile uint8_t t = (uart)->DR; UNUSED(t);
|
||||||
|
|
||||||
|
void uart_interrupt_handler(uart_ring *q) {
|
||||||
|
ENTER_CRITICAL();
|
||||||
|
|
||||||
|
// Read UART status. This is also the first step necessary in clearing most interrupts
|
||||||
|
uint32_t status = q->uart->SR;
|
||||||
|
|
||||||
|
// If RXNE is set, perform a read. This clears RXNE, ORE, IDLE, NF and FE
|
||||||
|
if((status & USART_SR_RXNE) != 0U){
|
||||||
|
uart_rx_ring(q);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Detect errors and clear them
|
||||||
|
uint32_t err = (status & USART_SR_ORE) | (status & USART_SR_NE) | (status & USART_SR_FE) | (status & USART_SR_PE);
|
||||||
|
if(err != 0U){
|
||||||
|
#ifdef DEBUG_UART
|
||||||
|
puts("Encountered UART error: "); puth(err); puts("\n");
|
||||||
|
#endif
|
||||||
|
UART_READ_DR(q->uart)
|
||||||
|
}
|
||||||
|
// Send if necessary
|
||||||
|
uart_tx_ring(q);
|
||||||
|
|
||||||
|
// Run DMA pointer handler if the line is idle
|
||||||
|
if(q->dma_rx && (status & USART_SR_IDLE)){
|
||||||
|
// Reset IDLE flag
|
||||||
|
UART_READ_DR(q->uart)
|
||||||
|
|
||||||
|
if(q == &uart_ring_esp_gps){
|
||||||
|
dma_pointer_handler(&uart_ring_esp_gps, DMA2_Stream5->NDTR);
|
||||||
|
} else {
|
||||||
|
#ifdef DEBUG_UART
|
||||||
|
puts("No IDLE dma_pointer_handler implemented for this UART.");
|
||||||
|
#endif
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
EXIT_CRITICAL();
|
EXIT_CRITICAL();
|
||||||
}
|
}
|
||||||
|
|
||||||
// interrupt boilerplate
|
void USART1_IRQHandler(void) { uart_interrupt_handler(&uart_ring_esp_gps); }
|
||||||
|
void USART2_IRQHandler(void) { uart_interrupt_handler(&uart_ring_debug); }
|
||||||
|
void USART3_IRQHandler(void) { uart_interrupt_handler(&uart_ring_lin2); }
|
||||||
|
void UART5_IRQHandler(void) { uart_interrupt_handler(&uart_ring_lin1); }
|
||||||
|
|
||||||
void USART1_IRQHandler(void) { uart_ring_process(&esp_ring); }
|
void DMA2_Stream5_IRQHandler(void) {
|
||||||
void USART2_IRQHandler(void) { uart_ring_process(&debug_ring); }
|
ENTER_CRITICAL();
|
||||||
void USART3_IRQHandler(void) { uart_ring_process(&lin2_ring); }
|
|
||||||
void UART5_IRQHandler(void) { uart_ring_process(&lin1_ring); }
|
// Handle errors
|
||||||
|
if((DMA2->HISR & DMA_HISR_TEIF5) || (DMA2->HISR & DMA_HISR_DMEIF5) || (DMA2->HISR & DMA_HISR_FEIF5)){
|
||||||
|
#ifdef DEBUG_UART
|
||||||
|
puts("Encountered UART DMA error. Clearing and restarting DMA...\n");
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// Clear flags
|
||||||
|
DMA2->HIFCR = DMA_HIFCR_CTEIF5 | DMA_HIFCR_CDMEIF5 | DMA_HIFCR_CFEIF5;
|
||||||
|
|
||||||
|
// Re-enable the DMA if necessary
|
||||||
|
DMA2_Stream5->CR |= DMA_SxCR_EN;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Re-calculate write pointer and reset flags
|
||||||
|
dma_pointer_handler(&uart_ring_esp_gps, DMA2_Stream5->NDTR);
|
||||||
|
DMA2->HIFCR = DMA_HIFCR_CTCIF5 | DMA_HIFCR_CHTIF5;
|
||||||
|
|
||||||
|
EXIT_CRITICAL();
|
||||||
|
}
|
||||||
|
|
||||||
|
// ***************************** Hardware setup *****************************
|
||||||
|
|
||||||
|
void dma_rx_init(uart_ring *q) {
|
||||||
|
// Initialization is UART-dependent
|
||||||
|
if(q == &uart_ring_esp_gps){
|
||||||
|
// DMA2, stream 5, channel 4
|
||||||
|
|
||||||
|
// Disable FIFO mode (enable direct)
|
||||||
|
DMA2_Stream5->FCR &= ~DMA_SxFCR_DMDIS;
|
||||||
|
|
||||||
|
// Setup addresses
|
||||||
|
DMA2_Stream5->PAR = (uint32_t)&(USART1->DR); // Source
|
||||||
|
DMA2_Stream5->M0AR = (uint32_t)q->elems_rx; // Destination
|
||||||
|
DMA2_Stream5->NDTR = q->rx_fifo_size; // Number of bytes to copy
|
||||||
|
|
||||||
|
// Circular, Increment memory, byte size, periph -> memory, enable
|
||||||
|
// Transfer complete, half transfer, transfer error and direct mode error interrupt enable
|
||||||
|
DMA2_Stream5->CR = DMA_SxCR_CHSEL_2 | DMA_SxCR_MINC | DMA_SxCR_CIRC | DMA_SxCR_HTIE | DMA_SxCR_TCIE | DMA_SxCR_TEIE | DMA_SxCR_DMEIE | DMA_SxCR_EN;
|
||||||
|
|
||||||
|
// Enable DMA receiver in UART
|
||||||
|
q->uart->CR3 |= USART_CR3_DMAR;
|
||||||
|
|
||||||
|
// Enable UART IDLE interrupt
|
||||||
|
q->uart->CR1 |= USART_CR1_IDLEIE;
|
||||||
|
|
||||||
|
// Enable interrupt
|
||||||
|
NVIC_EnableIRQ(DMA2_Stream5_IRQn);
|
||||||
|
} else {
|
||||||
|
puts("Tried to initialize RX DMA for an unsupported UART\n");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#define __DIV(_PCLK_, _BAUD_) (((_PCLK_) * 25U) / (4U * (_BAUD_)))
|
||||||
|
#define __DIVMANT(_PCLK_, _BAUD_) (__DIV((_PCLK_), (_BAUD_)) / 100U)
|
||||||
|
#define __DIVFRAQ(_PCLK_, _BAUD_) ((((__DIV((_PCLK_), (_BAUD_)) - (__DIVMANT((_PCLK_), (_BAUD_)) * 100U)) * 16U) + 50U) / 100U)
|
||||||
|
#define __USART_BRR(_PCLK_, _BAUD_) ((__DIVMANT((_PCLK_), (_BAUD_)) << 4) | (__DIVFRAQ((_PCLK_), (_BAUD_)) & 0x0FU))
|
||||||
|
|
||||||
|
void uart_set_baud(USART_TypeDef *u, unsigned int baud) {
|
||||||
|
if (u == USART1) {
|
||||||
|
// USART1 is on APB2
|
||||||
|
u->BRR = __USART_BRR(48000000U, baud);
|
||||||
|
} else {
|
||||||
|
u->BRR = __USART_BRR(24000000U, baud);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void uart_init(uart_ring *q, int baud) {
|
||||||
|
// Set baud and enable peripheral with TX and RX mode
|
||||||
|
uart_set_baud(q->uart, baud);
|
||||||
|
q->uart->CR1 = USART_CR1_UE | USART_CR1_TE | USART_CR1_RE;
|
||||||
|
|
||||||
|
// Enable UART interrupts
|
||||||
|
if(q->uart == USART1){
|
||||||
|
NVIC_EnableIRQ(USART1_IRQn);
|
||||||
|
} else if (q->uart == USART2){
|
||||||
|
NVIC_EnableIRQ(USART2_IRQn);
|
||||||
|
} else if (q->uart == USART3){
|
||||||
|
NVIC_EnableIRQ(USART3_IRQn);
|
||||||
|
} else if (q->uart == UART5){
|
||||||
|
NVIC_EnableIRQ(UART5_IRQn);
|
||||||
|
} else {
|
||||||
|
// UART not used. Skip enabling interrupts
|
||||||
|
}
|
||||||
|
|
||||||
|
// Initialise RX DMA if used
|
||||||
|
if(q->dma_rx){
|
||||||
|
dma_rx_init(q);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// ************************* Low-level buffer functions *************************
|
||||||
|
|
||||||
bool getc(uart_ring *q, char *elem) {
|
bool getc(uart_ring *q, char *elem) {
|
||||||
bool ret = false;
|
bool ret = false;
|
||||||
|
|
@ -124,7 +303,7 @@ bool getc(uart_ring *q, char *elem) {
|
||||||
ENTER_CRITICAL();
|
ENTER_CRITICAL();
|
||||||
if (q->w_ptr_rx != q->r_ptr_rx) {
|
if (q->w_ptr_rx != q->r_ptr_rx) {
|
||||||
if (elem != NULL) *elem = q->elems_rx[q->r_ptr_rx];
|
if (elem != NULL) *elem = q->elems_rx[q->r_ptr_rx];
|
||||||
q->r_ptr_rx = (q->r_ptr_rx + 1U) % FIFO_SIZE;
|
q->r_ptr_rx = (q->r_ptr_rx + 1U) % q->rx_fifo_size;
|
||||||
ret = true;
|
ret = true;
|
||||||
}
|
}
|
||||||
EXIT_CRITICAL();
|
EXIT_CRITICAL();
|
||||||
|
|
@ -137,7 +316,7 @@ bool injectc(uart_ring *q, char elem) {
|
||||||
uint16_t next_w_ptr;
|
uint16_t next_w_ptr;
|
||||||
|
|
||||||
ENTER_CRITICAL();
|
ENTER_CRITICAL();
|
||||||
next_w_ptr = (q->w_ptr_rx + 1U) % FIFO_SIZE;
|
next_w_ptr = (q->w_ptr_rx + 1U) % q->tx_fifo_size;
|
||||||
if (next_w_ptr != q->r_ptr_rx) {
|
if (next_w_ptr != q->r_ptr_rx) {
|
||||||
q->elems_rx[q->w_ptr_rx] = elem;
|
q->elems_rx[q->w_ptr_rx] = elem;
|
||||||
q->w_ptr_rx = next_w_ptr;
|
q->w_ptr_rx = next_w_ptr;
|
||||||
|
|
@ -153,7 +332,7 @@ bool putc(uart_ring *q, char elem) {
|
||||||
uint16_t next_w_ptr;
|
uint16_t next_w_ptr;
|
||||||
|
|
||||||
ENTER_CRITICAL();
|
ENTER_CRITICAL();
|
||||||
next_w_ptr = (q->w_ptr_tx + 1U) % FIFO_SIZE;
|
next_w_ptr = (q->w_ptr_tx + 1U) % q->tx_fifo_size;
|
||||||
if (next_w_ptr != q->r_ptr_tx) {
|
if (next_w_ptr != q->r_ptr_tx) {
|
||||||
q->elems_tx[q->w_ptr_tx] = elem;
|
q->elems_tx[q->w_ptr_tx] = elem;
|
||||||
q->w_ptr_tx = next_w_ptr;
|
q->w_ptr_tx = next_w_ptr;
|
||||||
|
|
@ -161,11 +340,13 @@ bool putc(uart_ring *q, char elem) {
|
||||||
}
|
}
|
||||||
EXIT_CRITICAL();
|
EXIT_CRITICAL();
|
||||||
|
|
||||||
uart_ring_process(q);
|
uart_tx_ring(q);
|
||||||
|
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Seems dangerous to use (might lock CPU if called with interrupts disabled f.e.)
|
||||||
|
// TODO: Remove? Not used anyways
|
||||||
void uart_flush(uart_ring *q) {
|
void uart_flush(uart_ring *q) {
|
||||||
while (q->w_ptr_tx != q->r_ptr_tx) {
|
while (q->w_ptr_tx != q->r_ptr_tx) {
|
||||||
__WFI();
|
__WFI();
|
||||||
|
|
@ -175,7 +356,7 @@ void uart_flush(uart_ring *q) {
|
||||||
void uart_flush_sync(uart_ring *q) {
|
void uart_flush_sync(uart_ring *q) {
|
||||||
// empty the TX buffer
|
// empty the TX buffer
|
||||||
while (q->w_ptr_tx != q->r_ptr_tx) {
|
while (q->w_ptr_tx != q->r_ptr_tx) {
|
||||||
uart_ring_process(q);
|
uart_tx_ring(q);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -193,119 +374,15 @@ void clear_uart_buff(uart_ring *q) {
|
||||||
EXIT_CRITICAL();
|
EXIT_CRITICAL();
|
||||||
}
|
}
|
||||||
|
|
||||||
// ***************************** start UART code *****************************
|
// ************************ High-level debug functions **********************
|
||||||
|
|
||||||
#define __DIV(_PCLK_, _BAUD_) (((_PCLK_) * 25U) / (4U * (_BAUD_)))
|
|
||||||
#define __DIVMANT(_PCLK_, _BAUD_) (__DIV((_PCLK_), (_BAUD_)) / 100U)
|
|
||||||
#define __DIVFRAQ(_PCLK_, _BAUD_) ((((__DIV((_PCLK_), (_BAUD_)) - (__DIVMANT((_PCLK_), (_BAUD_)) * 100U)) * 16U) + 50U) / 100U)
|
|
||||||
#define __USART_BRR(_PCLK_, _BAUD_) ((__DIVMANT((_PCLK_), (_BAUD_)) << 4) | (__DIVFRAQ((_PCLK_), (_BAUD_)) & 0x0FU))
|
|
||||||
|
|
||||||
void uart_set_baud(USART_TypeDef *u, unsigned int baud) {
|
|
||||||
if (u == USART1) {
|
|
||||||
// USART1 is on APB2
|
|
||||||
u->BRR = __USART_BRR(48000000U, baud);
|
|
||||||
} else {
|
|
||||||
u->BRR = __USART_BRR(24000000U, baud);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
#define USART1_DMA_LEN 0x20
|
|
||||||
char usart1_dma[USART1_DMA_LEN];
|
|
||||||
|
|
||||||
void uart_dma_drain(void) {
|
|
||||||
uart_ring *q = &esp_ring;
|
|
||||||
|
|
||||||
ENTER_CRITICAL();
|
|
||||||
|
|
||||||
if ((DMA2->HISR & DMA_HISR_TCIF5) || (DMA2->HISR & DMA_HISR_HTIF5) || (DMA2_Stream5->NDTR != USART1_DMA_LEN)) {
|
|
||||||
// disable DMA
|
|
||||||
q->uart->CR3 &= ~USART_CR3_DMAR;
|
|
||||||
DMA2_Stream5->CR &= ~DMA_SxCR_EN;
|
|
||||||
while ((DMA2_Stream5->CR & DMA_SxCR_EN) != 0);
|
|
||||||
|
|
||||||
unsigned int i;
|
|
||||||
for (i = 0; i < (USART1_DMA_LEN - DMA2_Stream5->NDTR); i++) {
|
|
||||||
char c = usart1_dma[i];
|
|
||||||
uint16_t next_w_ptr = (q->w_ptr_rx + 1U) % FIFO_SIZE;
|
|
||||||
if (next_w_ptr != q->r_ptr_rx) {
|
|
||||||
q->elems_rx[q->w_ptr_rx] = c;
|
|
||||||
q->w_ptr_rx = next_w_ptr;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// reset DMA len
|
|
||||||
DMA2_Stream5->NDTR = USART1_DMA_LEN;
|
|
||||||
|
|
||||||
// clear interrupts
|
|
||||||
DMA2->HIFCR = DMA_HIFCR_CTCIF5 | DMA_HIFCR_CHTIF5;
|
|
||||||
//DMA2->HIFCR = DMA_HIFCR_CTEIF5 | DMA_HIFCR_CDMEIF5 | DMA_HIFCR_CFEIF5;
|
|
||||||
|
|
||||||
// enable DMA
|
|
||||||
DMA2_Stream5->CR |= DMA_SxCR_EN;
|
|
||||||
q->uart->CR3 |= USART_CR3_DMAR;
|
|
||||||
}
|
|
||||||
|
|
||||||
EXIT_CRITICAL();
|
|
||||||
}
|
|
||||||
|
|
||||||
void DMA2_Stream5_IRQHandler(void) {
|
|
||||||
//set_led(LED_BLUE, 1);
|
|
||||||
uart_dma_drain();
|
|
||||||
//set_led(LED_BLUE, 0);
|
|
||||||
}
|
|
||||||
|
|
||||||
void uart_init(USART_TypeDef *u, int baud) {
|
|
||||||
// enable uart and tx+rx mode
|
|
||||||
u->CR1 = USART_CR1_UE;
|
|
||||||
uart_set_baud(u, baud);
|
|
||||||
|
|
||||||
u->CR1 |= USART_CR1_TE | USART_CR1_RE;
|
|
||||||
//u->CR2 = USART_CR2_STOP_0 | USART_CR2_STOP_1;
|
|
||||||
//u->CR2 = USART_CR2_STOP_0;
|
|
||||||
// ** UART is ready to work **
|
|
||||||
|
|
||||||
// enable interrupts
|
|
||||||
if (u != USART1) {
|
|
||||||
u->CR1 |= USART_CR1_RXNEIE;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (u == USART1) {
|
|
||||||
// DMA2, stream 2, channel 3
|
|
||||||
DMA2_Stream5->M0AR = (uint32_t)usart1_dma;
|
|
||||||
DMA2_Stream5->NDTR = USART1_DMA_LEN;
|
|
||||||
DMA2_Stream5->PAR = (uint32_t)&(USART1->DR);
|
|
||||||
|
|
||||||
// channel4, increment memory, periph -> memory, enable
|
|
||||||
DMA2_Stream5->CR = DMA_SxCR_CHSEL_2 | DMA_SxCR_MINC | DMA_SxCR_HTIE | DMA_SxCR_TCIE | DMA_SxCR_EN;
|
|
||||||
|
|
||||||
// this one uses DMA receiver
|
|
||||||
u->CR3 = USART_CR3_DMAR;
|
|
||||||
|
|
||||||
NVIC_EnableIRQ(DMA2_Stream5_IRQn);
|
|
||||||
NVIC_EnableIRQ(USART1_IRQn);
|
|
||||||
} else if (u == USART2) {
|
|
||||||
NVIC_EnableIRQ(USART2_IRQn);
|
|
||||||
} else if (u == USART3) {
|
|
||||||
NVIC_EnableIRQ(USART3_IRQn);
|
|
||||||
} else if (u == UART5) {
|
|
||||||
NVIC_EnableIRQ(UART5_IRQn);
|
|
||||||
} else {
|
|
||||||
// USART type undefined, skip
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void putch(const char a) {
|
void putch(const char a) {
|
||||||
if (has_external_debug_serial) {
|
if (has_external_debug_serial) {
|
||||||
/*while ((debug_ring.uart->SR & USART_SR_TXE) == 0);
|
|
||||||
debug_ring.uart->DR = a;*/
|
|
||||||
|
|
||||||
// assuming debugging is important if there's external serial connected
|
// assuming debugging is important if there's external serial connected
|
||||||
while (!putc(&debug_ring, a));
|
while (!putc(&uart_ring_debug, a));
|
||||||
|
|
||||||
//putc(&debug_ring, a);
|
|
||||||
} else {
|
} else {
|
||||||
// misra-c2012-17.7: serial debug function, ok to ignore output
|
// misra-c2012-17.7: serial debug function, ok to ignore output
|
||||||
(void)injectc(&debug_ring, a);
|
(void)injectc(&uart_ring_debug, a);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
13
board/main.c
13
board/main.c
|
|
@ -409,9 +409,6 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired)
|
||||||
if (!ur) {
|
if (!ur) {
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
if (ur == &esp_ring) {
|
|
||||||
uart_dma_drain();
|
|
||||||
}
|
|
||||||
// read
|
// read
|
||||||
while ((resp_len < MIN(setup->b.wLength.w, MAX_RESP_LEN)) &&
|
while ((resp_len < MIN(setup->b.wLength.w, MAX_RESP_LEN)) &&
|
||||||
getc(ur, (char*)&resp[resp_len])) {
|
getc(ur, (char*)&resp[resp_len])) {
|
||||||
|
|
@ -670,22 +667,22 @@ int main(void) {
|
||||||
if (has_external_debug_serial) {
|
if (has_external_debug_serial) {
|
||||||
// WEIRDNESS: without this gate around the UART, it would "crash", but only if the ESP is enabled
|
// WEIRDNESS: without this gate around the UART, it would "crash", but only if the ESP is enabled
|
||||||
// assuming it's because the lines were left floating and spurious noise was on them
|
// assuming it's because the lines were left floating and spurious noise was on them
|
||||||
uart_init(USART2, 115200);
|
uart_init(&uart_ring_debug, 115200);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (board_has_gps()) {
|
if (board_has_gps()) {
|
||||||
uart_init(USART1, 9600);
|
uart_init(&uart_ring_esp_gps, 9600);
|
||||||
} else {
|
} else {
|
||||||
// enable ESP uart
|
// enable ESP uart
|
||||||
uart_init(USART1, 115200);
|
uart_init(&uart_ring_esp_gps, 115200);
|
||||||
}
|
}
|
||||||
|
|
||||||
// there is no LIN on panda black
|
// there is no LIN on panda black
|
||||||
if(hw_type != HW_TYPE_BLACK_PANDA){
|
if(hw_type != HW_TYPE_BLACK_PANDA){
|
||||||
// enable LIN
|
// enable LIN
|
||||||
uart_init(UART5, 10400);
|
uart_init(&uart_ring_lin1, 10400);
|
||||||
UART5->CR2 |= USART_CR2_LINEN;
|
UART5->CR2 |= USART_CR2_LINEN;
|
||||||
uart_init(USART3, 10400);
|
uart_init(&uart_ring_lin2, 10400);
|
||||||
USART3->CR2 |= USART_CR2_LINEN;
|
USART3->CR2 |= USART_CR2_LINEN;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -84,9 +84,6 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired)
|
||||||
if (!ur) {
|
if (!ur) {
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
if (ur == &esp_ring) {
|
|
||||||
uart_dma_drain();
|
|
||||||
}
|
|
||||||
// read
|
// read
|
||||||
while ((resp_len < MIN(setup->b.wLength.w, MAX_RESP_LEN)) &&
|
while ((resp_len < MIN(setup->b.wLength.w, MAX_RESP_LEN)) &&
|
||||||
getc(ur, (char*)&resp[resp_len])) {
|
getc(ur, (char*)&resp[resp_len])) {
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,169 @@
|
||||||
|
#!/usr/bin/env python3
|
||||||
|
|
||||||
|
import os
|
||||||
|
import sys
|
||||||
|
import time
|
||||||
|
import random
|
||||||
|
import threading
|
||||||
|
|
||||||
|
sys.path.append(os.path.join(os.path.dirname(os.path.realpath(__file__)), ".."))
|
||||||
|
from panda import Panda, PandaSerial
|
||||||
|
|
||||||
|
INIT_GPS_BAUD = 9600
|
||||||
|
GPS_BAUD = 460800
|
||||||
|
|
||||||
|
def connect():
|
||||||
|
pandas = Panda.list()
|
||||||
|
print(pandas)
|
||||||
|
|
||||||
|
# make sure two pandas are connected
|
||||||
|
if len(pandas) != 2:
|
||||||
|
print("Connect white and grey/black panda to run this test!")
|
||||||
|
assert False
|
||||||
|
|
||||||
|
# connect
|
||||||
|
pandas[0] = Panda(pandas[0])
|
||||||
|
pandas[1] = Panda(pandas[1])
|
||||||
|
|
||||||
|
white_panda = None
|
||||||
|
gps_panda = None
|
||||||
|
|
||||||
|
# find out which one is white (for spamming the CAN buses)
|
||||||
|
if pandas[0].is_white() and not pandas[1].is_white():
|
||||||
|
white_panda = pandas[0]
|
||||||
|
gps_panda = pandas[1]
|
||||||
|
elif not pandas[0].is_white() and pandas[1].is_white():
|
||||||
|
white_panda = pandas[1]
|
||||||
|
gps_panda = pandas[0]
|
||||||
|
else:
|
||||||
|
print("Connect white and grey/black panda to run this test!")
|
||||||
|
assert False
|
||||||
|
return white_panda, gps_panda
|
||||||
|
|
||||||
|
def spam_buses_thread(panda):
|
||||||
|
try:
|
||||||
|
panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
|
||||||
|
while True:
|
||||||
|
at = random.randint(1, 2000)
|
||||||
|
st = (b"test"+os.urandom(10))[0:8]
|
||||||
|
bus = random.randint(0, 2)
|
||||||
|
panda.can_send(at, st, bus)
|
||||||
|
except Exception as e:
|
||||||
|
print(e)
|
||||||
|
|
||||||
|
def read_can_thread(panda):
|
||||||
|
try:
|
||||||
|
while True:
|
||||||
|
panda.can_recv()
|
||||||
|
except Exception as e:
|
||||||
|
print(e)
|
||||||
|
|
||||||
|
def init_gps(panda):
|
||||||
|
def add_nmea_checksum(msg):
|
||||||
|
d = msg[1:]
|
||||||
|
cs = 0
|
||||||
|
for i in d:
|
||||||
|
cs ^= ord(i)
|
||||||
|
return msg + "*%02X" % cs
|
||||||
|
|
||||||
|
ser = PandaSerial(panda, 1, INIT_GPS_BAUD)
|
||||||
|
|
||||||
|
# Power cycle the gps by toggling reset
|
||||||
|
print("Resetting GPS")
|
||||||
|
panda.set_esp_power(0)
|
||||||
|
time.sleep(0.5)
|
||||||
|
panda.set_esp_power(1)
|
||||||
|
time.sleep(0.5)
|
||||||
|
|
||||||
|
# Upping baud rate
|
||||||
|
print("Upping GPS baud rate")
|
||||||
|
msg = add_nmea_checksum("$PUBX,41,1,0007,0003,%d,0" % GPS_BAUD)+"\r\n"
|
||||||
|
ser.write(msg)
|
||||||
|
time.sleep(1) # needs a wait for it to actually send
|
||||||
|
|
||||||
|
# Reconnecting with the correct baud
|
||||||
|
ser = PandaSerial(panda, 1, GPS_BAUD)
|
||||||
|
|
||||||
|
# Sending all config messages boardd sends
|
||||||
|
print("Sending config")
|
||||||
|
ser.write("\xB5\x62\x06\x00\x14\x00\x03\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x01\x00\x00\x00\x00\x00\x1E\x7F")
|
||||||
|
ser.write("\xB5\x62\x06\x3E\x00\x00\x44\xD2")
|
||||||
|
ser.write("\xB5\x62\x06\x00\x14\x00\x00\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x19\x35")
|
||||||
|
ser.write("\xB5\x62\x06\x00\x14\x00\x01\x00\x00\x00\xC0\x08\x00\x00\x00\x08\x07\x00\x01\x00\x01\x00\x00\x00\x00\x00\xF4\x80")
|
||||||
|
ser.write("\xB5\x62\x06\x00\x14\x00\x04\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x1D\x85")
|
||||||
|
ser.write("\xB5\x62\x06\x00\x00\x00\x06\x18")
|
||||||
|
ser.write("\xB5\x62\x06\x00\x01\x00\x01\x08\x22")
|
||||||
|
ser.write("\xB5\x62\x06\x00\x01\x00\x02\x09\x23")
|
||||||
|
ser.write("\xB5\x62\x06\x00\x01\x00\x03\x0A\x24")
|
||||||
|
ser.write("\xB5\x62\x06\x08\x06\x00\x64\x00\x01\x00\x00\x00\x79\x10")
|
||||||
|
ser.write("\xB5\x62\x06\x24\x24\x00\x05\x00\x04\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x5A\x63")
|
||||||
|
ser.write("\xB5\x62\x06\x1E\x14\x00\x00\x00\x00\x00\x01\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x3C\x37")
|
||||||
|
ser.write("\xB5\x62\x06\x24\x00\x00\x2A\x84")
|
||||||
|
ser.write("\xB5\x62\x06\x23\x00\x00\x29\x81")
|
||||||
|
ser.write("\xB5\x62\x06\x1E\x00\x00\x24\x72")
|
||||||
|
ser.write("\xB5\x62\x06\x01\x03\x00\x01\x07\x01\x13\x51")
|
||||||
|
ser.write("\xB5\x62\x06\x01\x03\x00\x02\x15\x01\x22\x70")
|
||||||
|
ser.write("\xB5\x62\x06\x01\x03\x00\x02\x13\x01\x20\x6C")
|
||||||
|
|
||||||
|
print("Initialized GPS")
|
||||||
|
|
||||||
|
received_messages = 0
|
||||||
|
received_bytes = 0
|
||||||
|
send_something = False
|
||||||
|
def gps_read_thread(panda):
|
||||||
|
global received_messages, received_bytes, send_something
|
||||||
|
ser = PandaSerial(panda, 1, GPS_BAUD)
|
||||||
|
while True:
|
||||||
|
ret = ser.read(1024)
|
||||||
|
time.sleep(0.001)
|
||||||
|
l = len(ret)
|
||||||
|
if l > 0:
|
||||||
|
received_messages+=1
|
||||||
|
received_bytes+=l
|
||||||
|
if send_something:
|
||||||
|
ser.write("test")
|
||||||
|
send_something = False
|
||||||
|
|
||||||
|
|
||||||
|
CHECK_PERIOD = 5
|
||||||
|
MIN_BYTES = 10000
|
||||||
|
MAX_BYTES = 50000
|
||||||
|
|
||||||
|
min_failures = 0
|
||||||
|
max_failures = 0
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
white_panda, gps_panda = connect()
|
||||||
|
|
||||||
|
# Start spamming the CAN buses with the white panda. Also read the messages to add load on the GPS panda
|
||||||
|
threading.Thread(target=spam_buses_thread, args=(white_panda,)).start()
|
||||||
|
threading.Thread(target=read_can_thread, args=(gps_panda,)).start()
|
||||||
|
|
||||||
|
# Start GPS checking
|
||||||
|
init_gps(gps_panda)
|
||||||
|
|
||||||
|
read_thread = threading.Thread(target=gps_read_thread, args=(gps_panda,))
|
||||||
|
read_thread.start()
|
||||||
|
while True:
|
||||||
|
time.sleep(CHECK_PERIOD)
|
||||||
|
if(received_bytes < MIN_BYTES):
|
||||||
|
print("Panda is not sending out enough data! Got " + str(received_messages) + " (" + str(received_bytes) + "B) in the last " + str(CHECK_PERIOD) + " seconds")
|
||||||
|
send_something = True
|
||||||
|
min_failures+=1
|
||||||
|
elif(received_bytes > MAX_BYTES):
|
||||||
|
print("Panda is not sending out too much data! Got " + str(received_messages) + " (" + str(received_bytes) + "B) in the last " + str(CHECK_PERIOD) + " seconds")
|
||||||
|
print("Probably not on the right baud rate, got reset somehow? Resetting...")
|
||||||
|
max_failures+=1
|
||||||
|
init_gps(gps_panda)
|
||||||
|
else:
|
||||||
|
print("Got " + str(received_messages) + " (" + str(received_bytes) + "B) messages in the last " + str(CHECK_PERIOD) + " seconds.")
|
||||||
|
if(min_failures > 0):
|
||||||
|
print("Total min failures: ", min_failures)
|
||||||
|
if(max_failures > 0):
|
||||||
|
print("Total max failures: ", max_failures)
|
||||||
|
received_messages = 0
|
||||||
|
received_bytes = 0
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -0,0 +1,24 @@
|
||||||
|
#!/usr/bin/env python3
|
||||||
|
|
||||||
|
import os
|
||||||
|
import sys
|
||||||
|
import time
|
||||||
|
import random
|
||||||
|
|
||||||
|
sys.path.append(os.path.join(os.path.dirname(os.path.realpath(__file__)), ".."))
|
||||||
|
from panda import Panda
|
||||||
|
|
||||||
|
def get_test_string():
|
||||||
|
return b"test"+os.urandom(10)
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
p = Panda()
|
||||||
|
p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
|
||||||
|
|
||||||
|
print("Spamming all buses...")
|
||||||
|
while True:
|
||||||
|
at = random.randint(1, 2000)
|
||||||
|
st = get_test_string()[0:8]
|
||||||
|
bus = random.randint(0, 2)
|
||||||
|
p.can_send(at, st, bus)
|
||||||
|
#print("Sent message on bus: ", bus)
|
||||||
Loading…
Reference in New Issue