From 0ded9368457d366434b1bdc755f90213381b1228 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Tue, 21 Aug 2018 14:24:49 -0700 Subject: [PATCH 1/6] usb split buffers WIP --- CHANGELOG.md | 1 + .../Class/CDC/Inc/usbd_cdc.h | 28 ++++--- .../Class/CDC/Src/usbd_cdc.c | 76 +++++++++++-------- Firmware/Board/v3/Src/usbd_cdc_if.c | 31 ++++++-- 4 files changed, 88 insertions(+), 48 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index f024cb65..96e8644d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -24,6 +24,7 @@ Please add a note of your changes below this heading if you make a Pull Request. * `motor.config.requested_current_range` * `motor.config.current_control_bandwidth` and `motor.set_current_control_bandwidth`. Latter required to invoke gain recalculation. * `encoder.config.bandwidth` + * `sensorless_estimator.config.pm_flux_linkage` ## [0.4.1] - 2018-07-01 ### Fixed diff --git a/Firmware/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc/usbd_cdc.h b/Firmware/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc/usbd_cdc.h index 3bb73c6e..3f250554 100644 --- a/Firmware/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc/usbd_cdc.h +++ b/Firmware/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc/usbd_cdc.h @@ -109,19 +109,25 @@ typedef struct _USBD_CDC_Itf }USBD_CDC_ItfTypeDef; +typedef struct +{ + uint8_t* Buffer; + uint32_t Length; + volatile uint8_t State; +} +USBD_CDC_EP_HandleTypeDef; typedef struct { uint32_t data[CDC_DATA_HS_MAX_PACKET_SIZE/4]; /* Force 32bits alignment */ uint8_t CmdOpCode; - uint8_t CmdLength; - uint8_t *RxBuffer; - uint8_t *TxBuffer; - uint32_t RxLength; - uint32_t TxLength; - - __IO uint32_t TxState; - __IO uint32_t RxState; + uint8_t CmdLength; + + USBD_CDC_EP_HandleTypeDef CDC_Tx; + USBD_CDC_EP_HandleTypeDef CDC_Rx; + + USBD_CDC_EP_HandleTypeDef ODRIVE_Tx; + USBD_CDC_EP_HandleTypeDef ODRIVE_Rx; } USBD_CDC_HandleTypeDef; @@ -153,10 +159,12 @@ uint8_t USBD_CDC_RegisterInterface (USBD_HandleTypeDef *pdev, uint8_t USBD_CDC_SetTxBuffer (USBD_HandleTypeDef *pdev, uint8_t *pbuff, - uint16_t length); + uint16_t length, + uint8_t* odrivebuff, + uint16_t odrivebufflen); uint8_t USBD_CDC_SetRxBuffer (USBD_HandleTypeDef *pdev, - uint8_t *pbuff); + uint8_t *pbuff, uint8_t* odrivebuff); uint8_t USBD_CDC_ReceivePacket (USBD_HandleTypeDef *pdev, uint8_t endpoint_pair); diff --git a/Firmware/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c b/Firmware/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c index 2bc01513..6c21e45c 100644 --- a/Firmware/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c +++ b/Firmware/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c @@ -292,9 +292,9 @@ __ALIGN_BEGIN uint8_t USBD_CDC_CfgDesc[USB_CDC_CONFIG_DESC_SIZ] __ALIGN_END = 0x0B, /* bDescriptorType: Interface Association Descriptor */ 0x02, /* bFirstInterface */ 0x01, /* bInterfaceCount */ - 0x00, /* bFunctionClass: Communication Interface Class */ - 0x00, /* bFunctionSubClass: Abstract Control Model */ - 0x00, /* bFunctionProtocol: Common AT commands */ + 0x00, /* bFunctionClass: */ + 0x00, /* bFunctionSubClass: */ + 0x00, /* bFunctionProtocol: */ 0x06, /* iFunction */ /*---------------------------------------------------------------------------*/ @@ -414,15 +414,17 @@ static uint8_t USBD_CDC_Init (USBD_HandleTypeDef *pdev, ((USBD_CDC_ItfTypeDef *)pdev->pUserData)->Init(); /* Init Xfer states */ - hcdc->TxState =0; - hcdc->RxState =0; + hcdc->CDC_Tx.State =0; + hcdc->CDC_Rx.State =0; + hcdc->ODRIVE_Tx.State =0; + hcdc->ODRIVE_Rx.State =0; if(pdev->dev_speed == USBD_SPEED_HIGH ) { /* Prepare Out endpoint to receive next packet */ USBD_LL_PrepareReceive(pdev, CDC_OUT_EP, - hcdc->RxBuffer, + hcdc->CDC_Rx.Buffer, CDC_DATA_HS_OUT_PACKET_SIZE); } else @@ -430,14 +432,14 @@ static uint8_t USBD_CDC_Init (USBD_HandleTypeDef *pdev, /* Prepare Out endpoint to receive next packet */ USBD_LL_PrepareReceive(pdev, CDC_OUT_EP, - hcdc->RxBuffer, + hcdc->CDC_Rx.Buffer, CDC_DATA_FS_OUT_PACKET_SIZE); } /* Prepare ODrive Out endpoint to receive next packet */ USBD_LL_PrepareReceive(pdev, ODRIVE_OUT_EP, - hcdc->RxBuffer, + hcdc->ODRIVE_Rx.Buffer, CDC_DATA_FS_OUT_PACKET_SIZE); } return ret; @@ -568,8 +570,10 @@ static uint8_t USBD_CDC_DataIn (USBD_HandleTypeDef *pdev, uint8_t epnum) if(pdev->pClassData != NULL) { - - hcdc->TxState = 0; + if (epnum == CDC_IN_EP) + hcdc->CDC_Tx.State = 0; + if (epnum == ODRIVE_IN_EP) + hcdc->ODRIVE_Tx.State = 0; osSemaphoreRelease(sem_usb_tx); return USBD_OK; } @@ -710,12 +714,16 @@ uint8_t USBD_CDC_RegisterInterface (USBD_HandleTypeDef *pdev, */ uint8_t USBD_CDC_SetTxBuffer (USBD_HandleTypeDef *pdev, uint8_t *pbuff, - uint16_t length) + uint16_t length, + uint8_t* odrivebuff, + uint16_t odrivebufflen) { USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef*) pdev->pClassData; - hcdc->TxBuffer = pbuff; - hcdc->TxLength = length; + hcdc->CDC_Tx.Buffer = pbuff; + hcdc->CDC_Tx.Length = length; + hcdc->ODRIVE_Tx.Buffer = odrivebuff; + hcdc->ODRIVE_Tx.Length = odrivebufflen; return USBD_OK; } @@ -728,11 +736,12 @@ uint8_t USBD_CDC_SetTxBuffer (USBD_HandleTypeDef *pdev, * @retval status */ uint8_t USBD_CDC_SetRxBuffer (USBD_HandleTypeDef *pdev, - uint8_t *pbuff) + uint8_t *pbuff, uint8_t* odrivebuff) { USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef*) pdev->pClassData; - hcdc->RxBuffer = pbuff; + hcdc->CDC_Rx.Buffer = pbuff; + hcdc->ODRIVE_Rx.Buffer = odrivebuff; return USBD_OK; } @@ -750,25 +759,30 @@ uint8_t USBD_CDC_TransmitPacket(USBD_HandleTypeDef *pdev, uint8_t endpoint_pair if(pdev->pClassData != NULL) { - if(hcdc->TxState == 0) + // Select Endpoint + USBD_CDC_EP_HandleTypeDef* hEP_Tx; + uint8_t in_ep; + if (endpoint_pair == CDC_OUT_EP) { + hEP_Tx = hcdc->CDC_Tx; + in_ep = CDC_IN_EP; + } else if (endpoint_pair == ODRIVE_OUT_EP) { + hEP_Tx = hcdc->ODRIVE_Tx; + in_ep = ODRIVE_IN_EP; + } else { + return USBD_FAIL; + } + + if(hEP_Tx->State == 0) { /* Tx Transfer in progress */ - hcdc->TxState = 1; - - //endpoint_pair = 1; - if (endpoint_pair == 1) { - /* Transmit next packet */ - USBD_LL_Transmit(pdev, - CDC_IN_EP, - hcdc->TxBuffer, - hcdc->TxLength); - } else if (endpoint_pair == 3) { - USBD_LL_Transmit(pdev, - ODRIVE_IN_EP, - hcdc->TxBuffer, - hcdc->TxLength); - } + hEP_Tx->State = 1; + /* Transmit next packet */ + USBD_LL_Transmit(pdev, + in_ep, + hEP_Tx->Buffer, + hEP_Tx->Length); + return USBD_OK; } else diff --git a/Firmware/Board/v3/Src/usbd_cdc_if.c b/Firmware/Board/v3/Src/usbd_cdc_if.c index 77e70b2c..bcf1482d 100644 --- a/Firmware/Board/v3/Src/usbd_cdc_if.c +++ b/Firmware/Board/v3/Src/usbd_cdc_if.c @@ -117,10 +117,12 @@ /* Create buffer for reception and transmission */ /* It's up to user to redefine and/or remove those define */ /** Received data over USB are stored in this buffer */ -uint8_t UserRxBufferFS[APP_RX_DATA_SIZE]; +uint8_t CDCRxBufferFS[APP_RX_DATA_SIZE]; +uint8_t ODRIVERxBufferFS[APP_RX_DATA_SIZE]; /** Data to send over USB CDC are stored in this buffer */ -uint8_t UserTxBufferFS[APP_TX_DATA_SIZE]; +uint8_t CDCTxBufferFS[APP_TX_DATA_SIZE]; +uint8_t ODRIVETxBufferFS[APP_TX_DATA_SIZE]; /* USER CODE BEGIN PRIVATE_VARIABLES */ /* USER CODE END PRIVATE_VARIABLES */ @@ -315,14 +317,29 @@ uint8_t CDC_Transmit_FS(uint8_t* Buf, uint16_t Len, uint8_t endpoint_pair) //Check length if (Len > USB_TX_DATA_SIZE) return USBD_FAIL; - // Check for ongoing transmission + USBD_CDC_HandleTypeDef* hcdc = (USBD_CDC_HandleTypeDef*) hUsbDeviceFS.pClassData; - if (hcdc->TxState != 0) - return USBD_BUSY; + + // Select EP + USBD_CDC_EP_HandleTypeDef* hEP_Tx; + uint8_t* TxBuff; + if (endpoint_pair == CDC_OUT_EP) { + hEP_Tx = hcdc->CDC_Tx; + TxBuff = CDCTxBufferFS; + } else if (endpoint_pair == ODRIVE_OUT_EP) { + hEP_Tx = hcdc->ODRIVE_Tx; + TxBuff = ODRIVETxBufferFS; + } else { + return USBD_FAIL; + } + + // Check for ongoing transmission + if (hEP_Tx->State != 0) + return USBD_BUSY; // memcpy Buf into UserTxBufferFS - memcpy(UserTxBufferFS, Buf, Len); + memcpy(TxBuff, Buf, Len); // Update Len - USBD_CDC_SetTxBuffer(&hUsbDeviceFS, UserTxBufferFS, Len); + USBD_CDC_SetTxBuffer(&hUsbDeviceFS, TxBuff, Len); result = USBD_CDC_TransmitPacket(&hUsbDeviceFS, endpoint_pair); /* USER CODE END 7 */ return result; From c2cd26cc6ef3befc08452bf373f1ba87e529bcf5 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sat, 1 Sep 2018 19:29:49 -0700 Subject: [PATCH 2/6] change deadline style timeout for regular timeouts in USB and UART interfaces --- Firmware/communication/interface_uart.cpp | 7 ++++--- Firmware/communication/interface_usb.cpp | 11 ++++++----- tools/odrive/utils.py | 9 +++++---- 3 files changed, 15 insertions(+), 12 deletions(-) diff --git a/Firmware/communication/interface_uart.cpp b/Firmware/communication/interface_uart.cpp index 9eee9a0d..b3141138 100644 --- a/Firmware/communication/interface_uart.cpp +++ b/Firmware/communication/interface_uart.cpp @@ -19,7 +19,7 @@ static uint8_t dma_rx_buffer[UART_RX_BUFFER_SIZE]; static uint32_t dma_last_rcv_idx; // FIXME: the stdlib doesn't know about CMSIS threads, so this is just a global variable -static thread_local uint32_t deadline_ms = 0; +// static thread_local uint32_t deadline_ms = 0; osThreadId uart_thread; @@ -32,7 +32,8 @@ public: 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, 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); @@ -68,7 +69,7 @@ static void uart_server_thread(void * ctx) { // Fetch the circular buffer "write pointer", where it would write next uint32_t new_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR; - deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS); + // 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) { uart4_stream_input.process_bytes(dma_rx_buffer + dma_last_rcv_idx, diff --git a/Firmware/communication/interface_usb.cpp b/Firmware/communication/interface_usb.cpp index b864981e..0bff5d72 100644 --- a/Firmware/communication/interface_usb.cpp +++ b/Firmware/communication/interface_usb.cpp @@ -18,7 +18,7 @@ static uint32_t usb_len; static uint8_t active_endpoint_pair; // FIXME: the stdlib doesn't know about CMSIS threads, so this is just a global variable -static thread_local uint32_t deadline_ms = 0; +// static thread_local uint32_t deadline_ms = 0; osThreadId usb_thread; @@ -31,7 +31,8 @@ public: if (length > USB_TX_DATA_SIZE) return -1; // wait for USB interface to become ready - if (osSemaphoreWait(sem_usb_tx, deadline_to_timeout(deadline_ms)) != osOK) { + // if (osSemaphoreWait(sem_usb_tx, deadline_to_timeout(deadline_ms)) != osOK) { + if (osSemaphoreWait(sem_usb_tx, PROTOCOL_SERVER_TIMEOUT_MS) != osOK) { // If the host resets the device it might be that the TX-complete handler is never called // and the sem_usb_tx semaphore is never released. To handle this we just override the // TX buffer if this wait times out. The implication is that the channel is no longer lossless. @@ -85,11 +86,11 @@ static void usb_server_thread(void * ctx) { (void) ctx; for (;;) { - const uint32_t usb_check_timeout = 1; // ms - osStatus sem_stat = osSemaphoreWait(sem_usb_rx, usb_check_timeout); + // const uint32_t usb_check_timeout = 1; // ms + osStatus sem_stat = osSemaphoreWait(sem_usb_rx, osWaitForever); if (sem_stat == osOK) { usb_stats_.rx_cnt++; - deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS); + // deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS); if (active_endpoint_pair == CDC_OUT_EP && board_config.enable_ascii_protocol_on_usb) { ASCII_protocol_parse_stream(usb_buf, usb_len, usb_stream_output); } else { diff --git a/tools/odrive/utils.py b/tools/odrive/utils.py index 45249262..1b6c3089 100755 --- a/tools/odrive/utils.py +++ b/tools/odrive/utils.py @@ -109,8 +109,8 @@ def rate_test(device): Tests how many integers per second can be transmitted """ - import matplotlib.pyplot as plt - plt.ion() + # import matplotlib.pyplot as plt + # plt.ion() print("reading 10000 values...") numFrames = 10000 @@ -118,13 +118,14 @@ def rate_test(device): for _ in range(numFrames): vals.append(device.axis0.loop_counter) - plt.plot(vals) - loopsPerFrame = (vals[-1] - vals[0])/numFrames loopsPerSec = (168000000/(2*10192)) FramePerSec = loopsPerSec/loopsPerFrame print("Frames per second: " + str(FramePerSec)) + # plt.plot(vals) + # plt.show(block=True) + def usb_burn_in_test(get_var_callback, cancellation_token): """ Starts background threads that read a values form the USB device in a spin-loop From e26e93212cadce2f7a0104f46b5bd133fe1128dd Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 7 Sep 2018 19:44:00 -0700 Subject: [PATCH 3/6] first compile of new buffers --- Firmware/.vscode/c_cpp_properties.json | 1 + .../Class/CDC/Inc/usbd_cdc.h | 5 +- .../Class/CDC/Src/usbd_cdc.c | 82 ++++++++----- Firmware/Board/v3/Src/usbd_cdc_if.c | 14 ++- Firmware/MotorControl/controller.hpp | 7 +- Firmware/communication/interface_usb.cpp | 113 +++++++++++++----- Firmware/communication/interface_usb.h | 2 +- 7 files changed, 154 insertions(+), 70 deletions(-) diff --git a/Firmware/.vscode/c_cpp_properties.json b/Firmware/.vscode/c_cpp_properties.json index c77d5165..cb9d040e 100644 --- a/Firmware/.vscode/c_cpp_properties.json +++ b/Firmware/.vscode/c_cpp_properties.json @@ -29,6 +29,7 @@ "HW_VERSION_MAJOR=3", "HW_VERSION_MINOR=5", "HW_VERSION_VOLTAGE=24", + "USB_PROTOCOL_NATIVE", "__weak=\"__attribute__((weak))\"", "__packed=\"__attribute__((__packed__))\"", "__GNUC__" diff --git a/Firmware/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc/usbd_cdc.h b/Firmware/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc/usbd_cdc.h index 3f250554..c029e22a 100644 --- a/Firmware/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc/usbd_cdc.h +++ b/Firmware/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc/usbd_cdc.h @@ -160,11 +160,10 @@ uint8_t USBD_CDC_RegisterInterface (USBD_HandleTypeDef *pdev, uint8_t USBD_CDC_SetTxBuffer (USBD_HandleTypeDef *pdev, uint8_t *pbuff, uint16_t length, - uint8_t* odrivebuff, - uint16_t odrivebufflen); + uint8_t endpoint_pair); uint8_t USBD_CDC_SetRxBuffer (USBD_HandleTypeDef *pdev, - uint8_t *pbuff, uint8_t* odrivebuff); + uint8_t *pbuff, uint8_t endpoint_pair); uint8_t USBD_CDC_ReceivePacket (USBD_HandleTypeDef *pdev, uint8_t endpoint_pair); diff --git a/Firmware/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c b/Firmware/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c index 6c21e45c..631bb392 100644 --- a/Firmware/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c +++ b/Firmware/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c @@ -574,6 +574,7 @@ static uint8_t USBD_CDC_DataIn (USBD_HandleTypeDef *pdev, uint8_t epnum) hcdc->CDC_Tx.State = 0; if (epnum == ODRIVE_IN_EP) hcdc->ODRIVE_Tx.State = 0; + //Note: We could use independent semaphores for simoultainous USB transmission. osSemaphoreRelease(sem_usb_tx); return USBD_OK; } @@ -593,15 +594,24 @@ static uint8_t USBD_CDC_DataIn (USBD_HandleTypeDef *pdev, uint8_t epnum) static uint8_t USBD_CDC_DataOut (USBD_HandleTypeDef *pdev, uint8_t epnum) { USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef*) pdev->pClassData; + + USBD_CDC_EP_HandleTypeDef* hEP_Rx; + if (epnum == CDC_OUT_EP) { + hEP_Rx = &hcdc->CDC_Rx; + } else if (epnum == ODRIVE_OUT_EP) { + hEP_Rx = &hcdc->ODRIVE_Rx; + } else { + return USBD_FAIL; + } /* Get the received data length */ - hcdc->RxLength = USBD_LL_GetRxDataSize (pdev, epnum); + hEP_Rx->Length = USBD_LL_GetRxDataSize (pdev, epnum); /* USB data will be immediately processed, this allow next USB traffic being NAKed till the end of the application Xfer */ if(pdev->pClassData != NULL) { - ((USBD_CDC_ItfTypeDef *)pdev->pUserData)->Receive(hcdc->RxBuffer, &hcdc->RxLength, epnum); + ((USBD_CDC_ItfTypeDef *)pdev->pUserData)->Receive(hEP_Rx->Buffer, &hEP_Rx->Length, epnum); return USBD_OK; } @@ -715,15 +725,21 @@ uint8_t USBD_CDC_RegisterInterface (USBD_HandleTypeDef *pdev, uint8_t USBD_CDC_SetTxBuffer (USBD_HandleTypeDef *pdev, uint8_t *pbuff, uint16_t length, - uint8_t* odrivebuff, - uint16_t odrivebufflen) + uint8_t endpoint_pair) { USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef*) pdev->pClassData; - hcdc->CDC_Tx.Buffer = pbuff; - hcdc->CDC_Tx.Length = length; - hcdc->ODRIVE_Tx.Buffer = odrivebuff; - hcdc->ODRIVE_Tx.Length = odrivebufflen; + USBD_CDC_EP_HandleTypeDef* hEP_Tx; + if (endpoint_pair == CDC_OUT_EP) { + hEP_Tx = &hcdc->CDC_Tx; + } else if (endpoint_pair == ODRIVE_OUT_EP) { + hEP_Tx = &hcdc->ODRIVE_Tx; + } else { + return USBD_FAIL; + } + + hEP_Tx->Buffer = pbuff; + hEP_Tx->Length = length; return USBD_OK; } @@ -736,12 +752,20 @@ uint8_t USBD_CDC_SetTxBuffer (USBD_HandleTypeDef *pdev, * @retval status */ uint8_t USBD_CDC_SetRxBuffer (USBD_HandleTypeDef *pdev, - uint8_t *pbuff, uint8_t* odrivebuff) + uint8_t *pbuff, uint8_t endpoint_pair) { USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef*) pdev->pClassData; + + USBD_CDC_EP_HandleTypeDef* hEP_Rx; + if (endpoint_pair == CDC_OUT_EP) { + hEP_Rx = &hcdc->CDC_Rx; + } else if (endpoint_pair == ODRIVE_OUT_EP) { + hEP_Rx = &hcdc->ODRIVE_Rx; + } else { + return USBD_FAIL; + } - hcdc->CDC_Rx.Buffer = pbuff; - hcdc->ODRIVE_Rx.Buffer = odrivebuff; + hEP_Rx->Buffer = pbuff; return USBD_OK; } @@ -763,10 +787,10 @@ uint8_t USBD_CDC_TransmitPacket(USBD_HandleTypeDef *pdev, uint8_t endpoint_pair USBD_CDC_EP_HandleTypeDef* hEP_Tx; uint8_t in_ep; if (endpoint_pair == CDC_OUT_EP) { - hEP_Tx = hcdc->CDC_Tx; + hEP_Tx = &hcdc->CDC_Tx; in_ep = CDC_IN_EP; } else if (endpoint_pair == ODRIVE_OUT_EP) { - hEP_Tx = hcdc->ODRIVE_Tx; + hEP_Tx = &hcdc->ODRIVE_Tx; in_ep = ODRIVE_IN_EP; } else { return USBD_FAIL; @@ -810,23 +834,25 @@ uint8_t USBD_CDC_ReceivePacket(USBD_HandleTypeDef *pdev, uint8_t endpoint_pair) /* Suspend or Resume USB Out process */ if(pdev->pClassData != NULL) { - if (endpoint_pair == CDC_OUT_EP) - { - /* Prepare Out endpoint to receive next packet */ - USBD_LL_PrepareReceive(pdev, - CDC_OUT_EP, - hcdc->RxBuffer, - pdev->dev_speed == USBD_SPEED_HIGH ? CDC_DATA_HS_OUT_PACKET_SIZE : CDC_DATA_FS_OUT_PACKET_SIZE); - } - else if (endpoint_pair == ODRIVE_OUT_EP) - { - /* Prepare ODrive Out endpoint to receive next packet */ - USBD_LL_PrepareReceive(pdev, - ODRIVE_OUT_EP, - hcdc->RxBuffer, - pdev->dev_speed == USBD_SPEED_HIGH ? CDC_DATA_HS_OUT_PACKET_SIZE : CDC_DATA_FS_OUT_PACKET_SIZE); + // Select Endpoint + USBD_CDC_EP_HandleTypeDef* hEP_Rx; + uint8_t out_ep; + if (endpoint_pair == CDC_OUT_EP) { + hEP_Rx = &hcdc->CDC_Rx; + out_ep = CDC_OUT_EP; + } else if (endpoint_pair == ODRIVE_OUT_EP) { + hEP_Rx = &hcdc->ODRIVE_Rx; + out_ep = ODRIVE_OUT_EP; + } else { + return USBD_FAIL; } + /* Prepare Out endpoint to receive next packet */ + USBD_LL_PrepareReceive(pdev, + out_ep, + hEP_Rx->Buffer, + pdev->dev_speed == USBD_SPEED_HIGH ? CDC_DATA_HS_OUT_PACKET_SIZE : CDC_DATA_FS_OUT_PACKET_SIZE); + return USBD_OK; } else diff --git a/Firmware/Board/v3/Src/usbd_cdc_if.c b/Firmware/Board/v3/Src/usbd_cdc_if.c index bcf1482d..b8cb6e6c 100644 --- a/Firmware/Board/v3/Src/usbd_cdc_if.c +++ b/Firmware/Board/v3/Src/usbd_cdc_if.c @@ -179,8 +179,10 @@ static int8_t CDC_Init_FS(void) { /* USER CODE BEGIN 3 */ /* Set Application Buffers */ - USBD_CDC_SetTxBuffer(&hUsbDeviceFS, UserTxBufferFS, 0); - USBD_CDC_SetRxBuffer(&hUsbDeviceFS, UserRxBufferFS); + USBD_CDC_SetTxBuffer(&hUsbDeviceFS, CDCTxBufferFS, 0, CDC_OUT_EP); + USBD_CDC_SetRxBuffer(&hUsbDeviceFS, CDCRxBufferFS, CDC_OUT_EP); + USBD_CDC_SetTxBuffer(&hUsbDeviceFS, ODRIVETxBufferFS, 0, ODRIVE_OUT_EP); + USBD_CDC_SetRxBuffer(&hUsbDeviceFS, ODRIVERxBufferFS, ODRIVE_OUT_EP); return (USBD_OK); /* USER CODE END 3 */ } @@ -292,7 +294,7 @@ static int8_t CDC_Control_FS(uint8_t cmd, uint8_t* pbuf, uint16_t length) static int8_t CDC_Receive_FS(uint8_t* Buf, uint32_t *Len, uint8_t endpoint_pair) { /* USER CODE BEGIN 6 */ - usb_process_packet(Buf, *Len, endpoint_pair); + usb_rx_process_packet(Buf, *Len, endpoint_pair); return (USBD_OK); /* USER CODE END 6 */ @@ -324,10 +326,10 @@ uint8_t CDC_Transmit_FS(uint8_t* Buf, uint16_t Len, uint8_t endpoint_pair) USBD_CDC_EP_HandleTypeDef* hEP_Tx; uint8_t* TxBuff; if (endpoint_pair == CDC_OUT_EP) { - hEP_Tx = hcdc->CDC_Tx; + hEP_Tx = &hcdc->CDC_Tx; TxBuff = CDCTxBufferFS; } else if (endpoint_pair == ODRIVE_OUT_EP) { - hEP_Tx = hcdc->ODRIVE_Tx; + hEP_Tx = &hcdc->ODRIVE_Tx; TxBuff = ODRIVETxBufferFS; } else { return USBD_FAIL; @@ -339,7 +341,7 @@ uint8_t CDC_Transmit_FS(uint8_t* Buf, uint16_t Len, uint8_t endpoint_pair) // memcpy Buf into UserTxBufferFS memcpy(TxBuff, Buf, Len); // Update Len - USBD_CDC_SetTxBuffer(&hUsbDeviceFS, TxBuff, Len); + USBD_CDC_SetTxBuffer(&hUsbDeviceFS, TxBuff, Len, endpoint_pair); result = USBD_CDC_TransmitPacket(&hUsbDeviceFS, endpoint_pair); /* USER CODE END 7 */ return result; diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index f10b6211..80f5d25a 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -86,12 +86,9 @@ public: make_protocol_property("vel_limit", &config_.vel_limit) ), make_protocol_function("set_pos_setpoint", *this, &Controller::set_pos_setpoint, - "pos_setpoint", - "vel_feed_forward", - "current_feed_forward"), + "pos_setpoint", "vel_feed_forward", "current_feed_forward"), make_protocol_function("set_vel_setpoint", *this, &Controller::set_vel_setpoint, - "vel_setpoint", - "current_feed_forward"), + "vel_setpoint", "current_feed_forward"), make_protocol_function("set_current_setpoint", *this, &Controller::set_current_setpoint, "current_setpoint"), make_protocol_function("start_anticogging_calibration", *this, &Controller::start_anticogging_calibration) diff --git a/Firmware/communication/interface_usb.cpp b/Firmware/communication/interface_usb.cpp index 0bff5d72..ba1decc1 100644 --- a/Firmware/communication/interface_usb.cpp +++ b/Firmware/communication/interface_usb.cpp @@ -13,28 +13,22 @@ #include -static uint8_t* usb_buf; -static uint32_t usb_len; -static uint8_t active_endpoint_pair; - -// FIXME: the stdlib doesn't know about CMSIS threads, so this is just a global variable -// static thread_local uint32_t deadline_ms = 0; - osThreadId usb_thread; - USBStats_t usb_stats_ = {0}; class USBSender : public PacketSink { public: + USBSender(uint8_t endpoint_pair, osSemaphoreId sem_usb_tx) + : endpoint_pair_(endpoint_pair), sem_usb_tx_(sem_usb_tx) {} + int process_packet(const uint8_t* buffer, size_t length) { // cannot send partial packets if (length > USB_TX_DATA_SIZE) return -1; // wait for USB interface to become ready - // if (osSemaphoreWait(sem_usb_tx, deadline_to_timeout(deadline_ms)) != osOK) { - if (osSemaphoreWait(sem_usb_tx, PROTOCOL_SERVER_TIMEOUT_MS) != osOK) { + if (osSemaphoreWait(sem_usb_tx_, PROTOCOL_SERVER_TIMEOUT_MS) != osOK) { // If the host resets the device it might be that the TX-complete handler is never called - // and the sem_usb_tx semaphore is never released. To handle this we just override the + // and the sem_usb_tx_ semaphore is never released. To handle this we just override the // TX buffer if this wait times out. The implication is that the channel is no longer lossless. // TODO: handle endpoint reset properly usb_stats_.tx_overrun_cnt++; @@ -42,15 +36,22 @@ public: // transmit packet uint8_t status = CDC_Transmit_FS( const_cast(buffer) /* casting this const away is safe because... - well... it's not actually. Stupid STM. */, length, active_endpoint_pair); + well... it's not actually. Stupid STM. */, length, endpoint_pair_); if (status != USBD_OK) { - osSemaphoreRelease(sem_usb_tx); + osSemaphoreRelease(sem_usb_tx_); return -1; } usb_stats_.tx_cnt++; return 0; } -} usb_packet_output; +private: + uint8_t endpoint_pair_; + osSemaphoreId sem_usb_tx_; +}; + +// Note we could have independent semaphores here to allow concurrent transmission +USBSender usb_packet_output_cdc(CDC_OUT_EP, sem_usb_tx); +USBSender usb_packet_output_native(ODRIVE_OUT_EP, sem_usb_tx); class TreatPacketSinkAsStreamSink : public StreamSink { public: @@ -71,17 +72,48 @@ public: size_t get_free_space() { return SIZE_MAX; } private: PacketSink& output_; -} usb_stream_output(usb_packet_output); +} usb_stream_output(usb_packet_output_cdc); + +// This is used by the printf feature. Hence the above statics, and below seemingly random ptr (it's externed) +// TODO: less spaghetti code StreamSink* usb_stream_output_ptr = &usb_stream_output; #if defined(USB_PROTOCOL_NATIVE) -BidirectionalPacketBasedChannel usb_channel(usb_packet_output); +BidirectionalPacketBasedChannel usb_channel(usb_packet_output_native); #elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED) StreamBasedPacketSink usb_packetized_output(usb_stream_output); BidirectionalPacketBasedChannel usb_channel(usb_packetized_output); StreamToPacketSegmenter usb_native_stream_input(usb_channel); #endif +struct USBInterface { + uint8_t* rx_buf = nullptr; + uint32_t rx_len = 0; + bool data_pending = false; + uint8_t out_ep; + uint8_t in_ep; + USBSender& usb_sender; +}; + +// Note: statics make this less modular. +// Note: we use a single rx semaphore and loop over data_pending to allow a single pump loop thread +static USBInterface CDC_interface = { + .rx_buf = nullptr, + .rx_len = 0, + .data_pending = false, + .out_ep = CDC_OUT_EP, + .in_ep = CDC_IN_EP, + .usb_sender = usb_packet_output_cdc, +}; +static USBInterface ODrive_interface = { + .rx_buf = nullptr, + .rx_len = 0, + .data_pending = false, + .out_ep = ODRIVE_OUT_EP, + .in_ep = ODRIVE_IN_EP, + .usb_sender = usb_packet_output_native, +}; + static void usb_server_thread(void * ctx) { (void) ctx; @@ -90,27 +122,54 @@ static void usb_server_thread(void * ctx) { osStatus sem_stat = osSemaphoreWait(sem_usb_rx, osWaitForever); if (sem_stat == osOK) { usb_stats_.rx_cnt++; - // deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS); - if (active_endpoint_pair == CDC_OUT_EP && board_config.enable_ascii_protocol_on_usb) { - ASCII_protocol_parse_stream(usb_buf, usb_len, usb_stream_output); - } else { + + // CDC Interface + if (CDC_interface.data_pending) { + if (board_config.enable_ascii_protocol_on_usb) { + ASCII_protocol_parse_stream(CDC_interface.rx_buf, + CDC_interface.rx_len, usb_stream_output); + } else { #if defined(USB_PROTOCOL_NATIVE) - usb_channel.process_packet(usb_buf, usb_len); + usb_channel.process_packet(CDC_interface.rx_buf, CDC_interface.rx_len); #elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED) - usb_native_stream_input.process_bytes(usb_buf, usb_len, nullptr); + usb_native_stream_input.process_bytes( + CDC_interface.rx_buf, CDC_interface.rx_len, nullptr); #endif + } + USBD_CDC_ReceivePacket(&hUsbDeviceFS, CDC_interface.out_ep); // Allow next packet + } + + // Native Interface + if (ODrive_interface.data_pending) { +#if defined(USB_PROTOCOL_NATIVE) + usb_channel.process_packet(ODrive_interface.rx_buf, ODrive_interface.rx_len); +#elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED) + usb_native_stream_input.process_bytes( + ODrive_interface.rx_buf, ODrive_interface.rx_len, nullptr); +#endif + USBD_CDC_ReceivePacket(&hUsbDeviceFS, ODrive_interface.out_ep); // Allow next packet } - USBD_CDC_ReceivePacket(&hUsbDeviceFS, active_endpoint_pair); // Allow next packet } } } // Called from CDC_Receive_FS callback function, this allows the communication // thread to handle the incoming data -void usb_process_packet(uint8_t *buf, uint32_t len, uint8_t endpoint_pair) { - usb_buf = buf; - usb_len = len; - active_endpoint_pair = endpoint_pair; +void usb_rx_process_packet(uint8_t *buf, uint32_t len, uint8_t endpoint_pair) { + USBInterface* usb_iface; + if (endpoint_pair == CDC_interface.out_ep) { + usb_iface = &CDC_interface; + } else if (endpoint_pair == ODrive_interface.out_ep) { + usb_iface = &ODrive_interface; + } else { + return; + } + + // We don't allow the next USB packet until the previous one has been processed completely. + // Therefore it's safe to write to these vars directly since we know previous processing is complete. + usb_iface->rx_buf = buf; + usb_iface->rx_len = len; + usb_iface->data_pending = true; osSemaphoreRelease(sem_usb_rx); } diff --git a/Firmware/communication/interface_usb.h b/Firmware/communication/interface_usb.h index c4dabb99..0a5b94ff 100644 --- a/Firmware/communication/interface_usb.h +++ b/Firmware/communication/interface_usb.h @@ -21,7 +21,7 @@ typedef struct { extern USBStats_t usb_stats_; -void usb_process_packet(uint8_t *buf, uint32_t len, uint8_t endpoint_pair); +void usb_rx_process_packet(uint8_t *buf, uint32_t len, uint8_t endpoint_pair); void start_usb_server(void); #ifdef __cplusplus From a7ca3a7cbd04c6c8ec7f6ee37e6601448c37c1c6 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 7 Sep 2018 20:56:36 -0700 Subject: [PATCH 4/6] fix one of my bugs, and one of STM --- .../ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c | 5 +++-- Firmware/communication/interface_usb.cpp | 4 ++-- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/Firmware/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c b/Firmware/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c index 631bb392..73313522 100644 --- a/Firmware/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c +++ b/Firmware/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c @@ -570,9 +570,10 @@ static uint8_t USBD_CDC_DataIn (USBD_HandleTypeDef *pdev, uint8_t epnum) if(pdev->pClassData != NULL) { - if (epnum == CDC_IN_EP) + // NOTE: We would logically expect xx_IN_EP here, but we actually get the xx_OUT_EP + if (epnum == CDC_OUT_EP) hcdc->CDC_Tx.State = 0; - if (epnum == ODRIVE_IN_EP) + if (epnum == ODRIVE_OUT_EP) hcdc->ODRIVE_Tx.State = 0; //Note: We could use independent semaphores for simoultainous USB transmission. osSemaphoreRelease(sem_usb_tx); diff --git a/Firmware/communication/interface_usb.cpp b/Firmware/communication/interface_usb.cpp index ba1decc1..d6e38948 100644 --- a/Firmware/communication/interface_usb.cpp +++ b/Firmware/communication/interface_usb.cpp @@ -18,7 +18,7 @@ USBStats_t usb_stats_ = {0}; class USBSender : public PacketSink { public: - USBSender(uint8_t endpoint_pair, osSemaphoreId sem_usb_tx) + USBSender(uint8_t endpoint_pair, const osSemaphoreId& sem_usb_tx) : endpoint_pair_(endpoint_pair), sem_usb_tx_(sem_usb_tx) {} int process_packet(const uint8_t* buffer, size_t length) { @@ -46,7 +46,7 @@ public: } private: uint8_t endpoint_pair_; - osSemaphoreId sem_usb_tx_; + const osSemaphoreId& sem_usb_tx_; }; // Note we could have independent semaphores here to allow concurrent transmission From cf33cf2c141440540308a5950e21095fa57aaae4 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 7 Sep 2018 21:11:20 -0700 Subject: [PATCH 5/6] only process each packet once --- Firmware/communication/interface_usb.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Firmware/communication/interface_usb.cpp b/Firmware/communication/interface_usb.cpp index d6e38948..cd44c907 100644 --- a/Firmware/communication/interface_usb.cpp +++ b/Firmware/communication/interface_usb.cpp @@ -125,6 +125,7 @@ static void usb_server_thread(void * ctx) { // CDC Interface if (CDC_interface.data_pending) { + CDC_interface.data_pending = false; if (board_config.enable_ascii_protocol_on_usb) { ASCII_protocol_parse_stream(CDC_interface.rx_buf, CDC_interface.rx_len, usb_stream_output); @@ -141,6 +142,7 @@ static void usb_server_thread(void * ctx) { // Native Interface if (ODrive_interface.data_pending) { + ODrive_interface.data_pending = false; #if defined(USB_PROTOCOL_NATIVE) usb_channel.process_packet(ODrive_interface.rx_buf, ODrive_interface.rx_len); #elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED) From fb5cae841afa5240d2241ef20ce7ab9dcd3bada1 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 7 Sep 2018 21:24:51 -0700 Subject: [PATCH 6/6] update changelog --- CHANGELOG.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 96e8644d..32a214f2 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,6 +1,10 @@ # Unreleased Features Please add a note of your changes below this heading if you make a Pull Request. +# Unreleased +## Fixed +* Serious reliability issue with USB communication where packets on Native and the CDC interface would collide with each other. + # Releases ## [0.4.3] - 2018-08-30 ### Added