From 9486836886fe2b146500af2a34407d03f07f4881 Mon Sep 17 00:00:00 2001 From: robbederks Date: Fri, 4 Oct 2019 13:28:56 -0700 Subject: [PATCH] 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 --- board/config.h | 1 + board/drivers/uart.h | 419 +++++++++++++++++++++--------------- board/main.c | 13 +- board/pedal/main.c | 3 - tests/gps_stability_test.py | 169 +++++++++++++++ tests/spam_can.py | 24 +++ 6 files changed, 447 insertions(+), 182 deletions(-) create mode 100755 tests/gps_stability_test.py create mode 100755 tests/spam_can.py diff --git a/board/config.h b/board/config.h index 76b0c3c..c2eb412 100644 --- a/board/config.h +++ b/board/config.h @@ -2,6 +2,7 @@ #define PANDA_CONFIG_H //#define DEBUG +//#define DEBUG_UART //#define DEBUG_USB //#define DEBUG_SPI diff --git a/board/drivers/uart.h b/board/drivers/uart.h index 679f699..3a095f6 100644 --- a/board/drivers/uart.h +++ b/board/drivers/uart.h @@ -1,18 +1,43 @@ // IRQs: USART1, USART2, USART3, UART5 -#define FIFO_SIZE 0x400U +// ***************************** Definitions ***************************** +#define FIFO_SIZE_INT 0x400U +#define FIFO_SIZE_DMA 0x1000U + typedef struct uart_ring { volatile uint16_t w_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 r_ptr_rx; - uint8_t elems_rx[FIFO_SIZE]; + uint8_t *elems_rx; + uint32_t rx_fifo_size; USART_TypeDef *uart; void (*callback)(struct uart_ring*); + bool dma_rx; } 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 putc(uart_ring *q, char elem); @@ -21,48 +46,35 @@ void puts(const char *a); void puth(unsigned int i); void hexdump(const void *a, int l); +void debug_ring_callback(uart_ring *ring); -// ***************************** serial port queues ***************************** +// ******************************** UART buffers ******************************** -// esp = USART1 -uart_ring esp_ring = { .w_ptr_tx = 0, .r_ptr_tx = 0, - .w_ptr_rx = 0, .r_ptr_rx = 0, - .uart = USART1, - .callback = NULL}; +// esp_gps = USART1 +UART_BUFFER(esp_gps, FIFO_SIZE_DMA, FIFO_SIZE_INT, USART1, NULL, true) // lin1, K-LINE = UART5 // lin2, L-LINE = USART3 -uart_ring lin1_ring = { .w_ptr_tx = 0, .r_ptr_tx = 0, - .w_ptr_rx = 0, .r_ptr_rx = 0, - .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}; +UART_BUFFER(lin1, FIFO_SIZE_INT, FIFO_SIZE_INT, UART5, NULL, false) +UART_BUFFER(lin2, FIFO_SIZE_INT, FIFO_SIZE_INT, USART3, NULL, false) // debug = USART2 -void debug_ring_callback(uart_ring *ring); -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_BUFFER(debug, FIFO_SIZE_INT, FIFO_SIZE_INT, USART2, debug_ring_callback, false) uart_ring *get_ring_by_number(int a) { uart_ring *ring = NULL; switch(a) { case 0: - ring = &debug_ring; + ring = &uart_ring_debug; break; case 1: - ring = &esp_ring; + ring = &uart_ring_esp_gps; break; case 2: - ring = &lin1_ring; + ring = &uart_ring_lin1; break; case 3: - ring = &lin2_ring; + ring = &uart_ring_lin2; break; default: ring = NULL; @@ -71,52 +83,219 @@ uart_ring *get_ring_by_number(int a) { return ring; } -// ***************************** serial port ***************************** +// ***************************** Interrupt handlers ***************************** -void uart_ring_process(uart_ring *q) { +void uart_tx_ring(uart_ring *q){ ENTER_CRITICAL(); - // TODO: check if external serial is connected - int sr = q->uart->SR; - + // Send out next byte of TX buffer if (q->w_ptr_tx != q->r_ptr_tx) { - if ((sr & USART_SR_TXE) != 0) { - q->uart->DR = q->elems_tx[q->r_ptr_tx]; - q->r_ptr_tx = (q->r_ptr_tx + 1U) % FIFO_SIZE; + // Only send if transmit register is empty (aka last byte has been sent) + if ((q->uart->SR & USART_SR_TXE) != 0) { + 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)) { - uint8_t c = q->uart->DR; // TODO: can drop packets - if (q != &esp_ring) { - 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; - if (q->callback != NULL) { - q->callback(q); - } + // Enable TXE interrupt if there is still data to be sent + if(q->r_ptr_tx != q->w_ptr_tx){ + q->uart->CR1 |= USART_CR1_TXEIE; + } else { + q->uart->CR1 &= ~USART_CR1_TXEIE; + } + } + EXIT_CRITICAL(); +} + +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) { - // set dropped packet flag? + prev_w_index = w_index; + 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(); } -// 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 USART2_IRQHandler(void) { uart_ring_process(&debug_ring); } -void USART3_IRQHandler(void) { uart_ring_process(&lin2_ring); } -void UART5_IRQHandler(void) { uart_ring_process(&lin1_ring); } +void DMA2_Stream5_IRQHandler(void) { + ENTER_CRITICAL(); + + // 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 ret = false; @@ -124,7 +303,7 @@ bool getc(uart_ring *q, char *elem) { ENTER_CRITICAL(); if (q->w_ptr_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; } EXIT_CRITICAL(); @@ -137,7 +316,7 @@ bool injectc(uart_ring *q, char elem) { uint16_t next_w_ptr; 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) { q->elems_rx[q->w_ptr_rx] = elem; q->w_ptr_rx = next_w_ptr; @@ -153,7 +332,7 @@ bool putc(uart_ring *q, char elem) { uint16_t next_w_ptr; 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) { q->elems_tx[q->w_ptr_tx] = elem; q->w_ptr_tx = next_w_ptr; @@ -161,11 +340,13 @@ bool putc(uart_ring *q, char elem) { } EXIT_CRITICAL(); - uart_ring_process(q); + uart_tx_ring(q); 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) { while (q->w_ptr_tx != q->r_ptr_tx) { __WFI(); @@ -175,7 +356,7 @@ void uart_flush(uart_ring *q) { void uart_flush_sync(uart_ring *q) { // empty the TX buffer 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(); } -// ***************************** start UART code ***************************** - -#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 - } -} - +// ************************ High-level debug functions ********************** void putch(const char a) { 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 - while (!putc(&debug_ring, a)); + while (!putc(&uart_ring_debug, a)); - //putc(&debug_ring, a); } else { // misra-c2012-17.7: serial debug function, ok to ignore output - (void)injectc(&debug_ring, a); + (void)injectc(&uart_ring_debug, a); } } diff --git a/board/main.c b/board/main.c index d87425c..b8cacf5 100644 --- a/board/main.c +++ b/board/main.c @@ -409,9 +409,6 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired) if (!ur) { break; } - if (ur == &esp_ring) { - uart_dma_drain(); - } // read while ((resp_len < MIN(setup->b.wLength.w, MAX_RESP_LEN)) && getc(ur, (char*)&resp[resp_len])) { @@ -670,22 +667,22 @@ int main(void) { if (has_external_debug_serial) { // 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 - uart_init(USART2, 115200); + uart_init(&uart_ring_debug, 115200); } if (board_has_gps()) { - uart_init(USART1, 9600); + uart_init(&uart_ring_esp_gps, 9600); } else { // enable ESP uart - uart_init(USART1, 115200); + uart_init(&uart_ring_esp_gps, 115200); } // there is no LIN on panda black if(hw_type != HW_TYPE_BLACK_PANDA){ // enable LIN - uart_init(UART5, 10400); + uart_init(&uart_ring_lin1, 10400); UART5->CR2 |= USART_CR2_LINEN; - uart_init(USART3, 10400); + uart_init(&uart_ring_lin2, 10400); USART3->CR2 |= USART_CR2_LINEN; } diff --git a/board/pedal/main.c b/board/pedal/main.c index b6cfa51..e7642f0 100644 --- a/board/pedal/main.c +++ b/board/pedal/main.c @@ -84,9 +84,6 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired) if (!ur) { break; } - if (ur == &esp_ring) { - uart_dma_drain(); - } // read while ((resp_len < MIN(setup->b.wLength.w, MAX_RESP_LEN)) && getc(ur, (char*)&resp[resp_len])) { diff --git a/tests/gps_stability_test.py b/tests/gps_stability_test.py new file mode 100755 index 0000000..c96aaf1 --- /dev/null +++ b/tests/gps_stability_test.py @@ -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 + + + + \ No newline at end of file diff --git a/tests/spam_can.py b/tests/spam_can.py new file mode 100755 index 0000000..6b0c2d9 --- /dev/null +++ b/tests/spam_can.py @@ -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) \ No newline at end of file