From: LeonardoBizzoni Date: Sat, 21 Feb 2026 15:42:13 +0000 (+0100) Subject: UART transmission error reporting X-Git-Url: http://git.leonardobizzoni.com/?a=commitdiff_plain;h=4065b13af74eaa66681a01368a7f68d0b2b1073b;p=pioneer-stm32 UART transmission error reporting --- diff --git a/pioneer_controller/Core/Inc/firmware/fmw_core.h b/pioneer_controller/Core/Inc/firmware/fmw_core.h index 74e99a4..2d84ee7 100644 --- a/pioneer_controller/Core/Inc/firmware/fmw_core.h +++ b/pioneer_controller/Core/Inc/firmware/fmw_core.h @@ -103,6 +103,8 @@ void fmw_buzzer_set(FMW_Buzzer buzzer[], int32_t count, bool on) __attribute__(( FMW_Result fmw_message_uart_receive(UART_HandleTypeDef *huart, FMW_Message *msg, int32_t wait_ms) __attribute__((warn_unused_result, nonnull)); void fmw_message_uart_send(UART_HandleTypeDef *huart, CRC_HandleTypeDef *hcrc, FMW_Message *msg, int32_t wait_ms) __attribute__((nonnull)); +FMW_Message fmw_message_from_uart_error(const UART_HandleTypeDef *huart); + #define FMW_LED_UPDATE_PERIOD 200 #define FMW_DEBOUNCE_DELAY 200 diff --git a/pioneer_controller/Core/Inc/firmware/fmw_messages.h b/pioneer_controller/Core/Inc/firmware/fmw_messages.h index be39bce..74f8172 100644 --- a/pioneer_controller/Core/Inc/firmware/fmw_messages.h +++ b/pioneer_controller/Core/Inc/firmware/fmw_messages.h @@ -10,46 +10,49 @@ typedef union { float values[3]; } FMW_PidConstants; +#define FMW_MESSAGE_TYPE_VARIANTS(X) \ + X(FMW_MessageType_None) \ + X(FMW_MessageType_Response) \ + X(FMW_MessageType_StateChange_Init) \ + X(FMW_MessageType_StateChange_Run) \ + X(FMW_MessageType_Config_Robot) \ + X(FMW_MessageType_Config_PID) \ + X(FMW_MessageType_Config_LED) \ + X(FMW_MessageType_Run_GetStatus) \ + X(FMW_MessageType_Run_GetStatus_Response) \ + X(FMW_MessageType_Run_SetVelocity) \ + X(FMW_MessageType_Run_SetVelocity_Response) \ + X(FMW_MessageType_COUNT) + typedef uint8_t FMW_MessageType; enum { - FMW_MessageType_None, - - FMW_MessageType_Error, - FMW_MessageType_Response, - - FMW_MessageType_StateChange_Init, - FMW_MessageType_StateChange_Run, - - FMW_MessageType_Config_Robot, - FMW_MessageType_Config_PID, - FMW_MessageType_Config_LED, - - FMW_MessageType_Run_GetStatus, - FMW_MessageType_Run_GetStatus_Response, - FMW_MessageType_Run_SetVelocity, - FMW_MessageType_Run_SetVelocity_Response, - - FMW_MessageType_COUNT, +#define X(Variant) Variant, + FMW_MESSAGE_TYPE_VARIANTS(X) +#undef X }; -#define FMW_RESULT_VARIANTS(X) \ - X(FMW_Result_Ok) \ - X(FMW_Result_Error_InvalidArguments) \ - X(FMW_Result_Error_UART_Crc) \ - X(FMW_Result_Error_UART_NegativeTimeout) \ - X(FMW_Result_Error_UART_ReceiveTimeoutElapsed) \ - X(FMW_Result_Error_Encoder_InvalidTimer) \ - X(FMW_Result_Error_Encoder_NonPositiveTicksPerRevolution) \ - X(FMW_Result_Error_Encoder_NonPositiveWheelCircumference) \ - X(FMW_Result_Error_Encoder_GetTick) \ - X(FMW_Result_Error_Buzzer_Timer) \ - X(FMW_Result_Error_MessageHandler_InvalidState) \ - X(FMW_Result_Error_MessageHandler_Init_NonPositiveBaseline) \ - X(FMW_Result_Error_MessageHandler_Init_NonPositiveWheelCircumference) \ - X(FMW_Result_Error_MessageHandler_Init_NonPositiveTicksPerRevolution) \ - X(FMW_Result_Error_MessageHandler_Init_NonPositiveLEDUpdatePeriod) \ - X(FMW_Result_Error_Command_NotRecognized) \ - X(FMW_Result_Error_Command_NotAvailable) \ +#define FMW_RESULT_VARIANTS(X) \ + X(FMW_Result_Ok) \ + X(FMW_Result_Error_InvalidArguments) \ + X(FMW_Result_Error_UART_Crc) \ + X(FMW_Result_Error_UART_NegativeTimeout) \ + X(FMW_Result_Error_UART_ReceiveTimeoutElapsed) \ + X(FMW_Result_Error_UART_Parity) \ + X(FMW_Result_Error_UART_Frame) \ + X(FMW_Result_Error_UART_Noise) \ + X(FMW_Result_Error_UART_Overrun) \ + X(FMW_Result_Error_Encoder_InvalidTimer) \ + X(FMW_Result_Error_Encoder_NonPositiveTicksPerRevolution) \ + X(FMW_Result_Error_Encoder_NonPositiveWheelCircumference) \ + X(FMW_Result_Error_Encoder_GetTick) \ + X(FMW_Result_Error_Buzzer_Timer) \ + X(FMW_Result_Error_MessageHandler_InvalidState) \ + X(FMW_Result_Error_MessageHandler_Init_NonPositiveBaseline) \ + X(FMW_Result_Error_MessageHandler_Init_NonPositiveWheelCircumference) \ + X(FMW_Result_Error_MessageHandler_Init_NonPositiveTicksPerRevolution) \ + X(FMW_Result_Error_MessageHandler_Init_NonPositiveLEDUpdatePeriod) \ + X(FMW_Result_Error_Command_NotRecognized) \ + X(FMW_Result_Error_Command_NotAvailable) \ X(FMW_Result_COUNT) typedef uint8_t FMW_Result; @@ -67,11 +70,6 @@ typedef struct { } header; union { - struct { - int32_t line; - int32_t file_size; - } error; - struct { float baseline; float wheel_circumference_left; @@ -91,7 +89,7 @@ typedef struct { uint32_t update_period; } config_led; - FMW_Result result; + FMW_Result response; struct { FMW_Result result; uint16_t delta_millis; diff --git a/pioneer_controller/Core/Src/firmware/fwm_core.c b/pioneer_controller/Core/Src/firmware/fwm_core.c index 6042b5a..68f6499 100644 --- a/pioneer_controller/Core/Src/firmware/fwm_core.c +++ b/pioneer_controller/Core/Src/firmware/fwm_core.c @@ -32,6 +32,33 @@ void fmw_message_uart_send(UART_HandleTypeDef *huart, CRC_HandleTypeDef *hcrc, F FMW_ASSERT(res == HAL_OK, .callback = fmw_hook_assert_fail); } +FMW_Message fmw_message_from_uart_error(const UART_HandleTypeDef *huart) { + FMW_ASSERT(huart->ErrorCode != HAL_UART_ERROR_NONE, .callback = fmw_hook_assert_fail); + FMW_Message res = {0}; + res.header.type = FMW_MessageType_Response; + switch (huart->ErrorCode) { + case HAL_UART_ERROR_PE: { + res.response = FMW_Result_Error_UART_Parity; + } break; + case HAL_UART_ERROR_FE: { + res.response = FMW_Result_Error_UART_Frame; + } break; + case HAL_UART_ERROR_NE: { + res.response = FMW_Result_Error_UART_Noise; + } break; + case HAL_UART_ERROR_ORE: { + res.response = FMW_Result_Error_UART_Overrun; + } break; + case HAL_UART_ERROR_RTO: { + res.response = FMW_Result_Error_UART_ReceiveTimeoutElapsed; + } break; + default: { // NOTE(lb): unreachable + FMW_ASSERT(false, .callback = fmw_hook_assert_fail); + } break; + } + return res; +} + // ============================================================ // Motor controller void fmw_motor_init(FMW_Motor motors[], int32_t count) { diff --git a/pioneer_controller/Core/Src/main.c b/pioneer_controller/Core/Src/main.c index 1a1b1ca..db12543 100644 --- a/pioneer_controller/Core/Src/main.c +++ b/pioneer_controller/Core/Src/main.c @@ -815,11 +815,18 @@ void start(void) { HAL_StatusTypeDef timer_status = HAL_TIM_Base_Start_IT(&htim6); FMW_ASSERT(timer_status == HAL_OK); - for (uint32_t time_last_led_update = 0;;) { - uint32_t time_now = HAL_GetTick(); - if (time_now - time_last_led_update >= led_update_period) { - time_last_led_update = time_now; - fmw_led_update(&pled); + for (;;) { + switch (current_mode) { + case FMW_Mode_Init: { + } break; + case FMW_Mode_Run: { + static uint32_t time_last_led_update = 0; + uint32_t time_now = HAL_GetTick(); + if (time_now - time_last_led_update >= led_update_period) { + time_last_led_update = time_now; + fmw_led_update(&pled); + } + } break; } } } @@ -929,6 +936,8 @@ FMW_Result message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) { // TIMER 100Hz PID control void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { + if (htim != &htim6) { return; } + // NOTE(lb): metrics taken for transmission ticks_left += fmw_encoder_count_get(&encoders.left); ticks_right += fmw_encoder_count_get(&encoders.right); @@ -952,18 +961,22 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { } void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) { - if (huart == UART_MESSANGER_HANDLE) { - FMW_Message response = {0}; - response.header.type = FMW_MessageType_Response; - response.result = message_handler((FMW_Message*)&uart_message_buffer, &hcrc); - fmw_message_uart_send(UART_MESSANGER_HANDLE, &hcrc, &response, HAL_MAX_DELAY); - // NOTE(lb): listen for the next message. - HAL_UART_Receive_DMA(UART_MESSANGER_HANDLE, (uint8_t*)&uart_message_buffer, sizeof uart_message_buffer); - } + if (huart != UART_MESSANGER_HANDLE) { return; } + FMW_Message response = {0}; + response.header.type = FMW_MessageType_Response; + response.response = message_handler((FMW_Message*)&uart_message_buffer, &hcrc); + fmw_message_uart_send(UART_MESSANGER_HANDLE, &hcrc, &response, HAL_MAX_DELAY); + // NOTE(lb): listen for the next message. + HAL_UART_Receive_DMA(UART_MESSANGER_HANDLE, (uint8_t*)&uart_message_buffer, sizeof uart_message_buffer); } void HAL_UART_ErrorCallback(UART_HandleTypeDef *huart) { - FMW_ASSERT(false); + if (huart != UART_MESSANGER_HANDLE) { return; } + FMW_Message response = fmw_message_from_uart_error(huart); + fmw_message_uart_send(UART_MESSANGER_HANDLE, &hcrc, &response, HAL_MAX_DELAY); + + HAL_UART_AbortReceive(huart); + HAL_UART_Receive_DMA(UART_MESSANGER_HANDLE, (uint8_t*)&uart_message_buffer, sizeof uart_message_buffer); } void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { diff --git a/pioneer_controller/pioneer_controller.ioc b/pioneer_controller/pioneer_controller.ioc index ab3cea3..175b7bc 100644 --- a/pioneer_controller/pioneer_controller.ioc +++ b/pioneer_controller/pioneer_controller.ioc @@ -243,7 +243,7 @@ ProjectManager.ToolChainLocation= ProjectManager.UAScriptAfterPath= ProjectManager.UAScriptBeforePath= ProjectManager.UnderRoot=true -ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,4-MX_USART3_UART_Init-USART3-false-HAL-true,5-MX_ADC1_Init-ADC1-false-HAL-true,6-MX_TIM4_Init-TIM4-false-HAL-true,7-MX_TIM1_Init-TIM1-false-HAL-true,8-MX_TIM2_Init-TIM2-false-HAL-true,9-MX_TIM3_Init-TIM3-false-HAL-true,10-MX_TIM6_Init-TIM6-false-HAL-true,11-MX_CRC_Init-CRC-false-HAL-true,0-MX_DMA_Init-DMA-false-HAL-true +ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,4-MX_USART3_UART_Init-USART3-false-HAL-true,5-MX_ADC1_Init-ADC1-false-HAL-true,6-MX_TIM4_Init-TIM4-false-HAL-true,7-MX_TIM1_Init-TIM1-false-HAL-true,8-MX_TIM2_Init-TIM2-false-HAL-true,9-MX_TIM3_Init-TIM3-false-HAL-true,10-MX_TIM6_Init-TIM6-false-HAL-true,0-MX_CORTEX_M7_Init-CORTEX_M7-false-HAL-true,0-MX_DMA_Init-DMA-false-HAL-true RCC.48MHZClocksFreq_Value=24000000 RCC.ADC12outputFreq_Value=72000000 RCC.ADC34outputFreq_Value=72000000 diff --git a/pioneer_workstation/src/main.c b/pioneer_workstation/src/main.c index 5bdf32f..5ff24c8 100644 --- a/pioneer_workstation/src/main.c +++ b/pioneer_workstation/src/main.c @@ -39,6 +39,35 @@ static int serial_open(char *portname) { return fd; } +static void pprint_message(const FMW_Message *msg) { + static const char *message_types[] = { +#define X(Variant) #Variant, + FMW_MESSAGE_TYPE_VARIANTS(X) +#undef X + }; + + static const char *result_types[] = { +#define X(Variant) #Variant, + FMW_RESULT_VARIANTS(X) +#undef X + }; + + printf("FMW_Message {" + "\n header.type = %s" + "\n header.crc = %u", + message_types[msg->header.type], + msg->header.crc); + switch (msg->header.type) { + case FMW_MessageType_Response: { + printf("\n response: %s", result_types[msg->response]); + } break; + default: { + assert(0 && "unreachable"); + } break; + } + printf("\n}\n"); +} + int main(void) { int fd = serial_open("/dev/ttyACM0"); assert(fd); @@ -64,7 +93,8 @@ int main(void) { if (n >= 0) { bytes_read += (uint32_t)n; } } assert(response.header.type == FMW_MessageType_Response); - assert(response.result == FMW_Result_Ok); + pprint_message(&response); + assert(response.response == FMW_Result_Ok); FMW_Message msg_config_pid = { .header = { @@ -95,7 +125,8 @@ int main(void) { if (n >= 0) { bytes_read += (uint32_t)n; } } assert(response.header.type == FMW_MessageType_Response); - assert(response.result == FMW_Result_Ok); + pprint_message(&response); + assert(response.response == FMW_Result_Ok); FMW_Message msg_run = { .header = { @@ -109,7 +140,8 @@ int main(void) { if (n >= 0) { bytes_read += (uint32_t)n; } } assert(response.header.type == FMW_MessageType_Response); - assert(response.result == FMW_Result_Ok); + pprint_message(&response); + assert(response.response == FMW_Result_Ok); close(fd); }