From: LeonardoBizzoni Date: Sun, 22 Feb 2026 14:41:08 +0000 (+0100) Subject: Status response to SetVelocity messages + improved UART error code X-Git-Url: http://git.leonardobizzoni.com/?a=commitdiff_plain;h=22498f36b2798e6e0324b73500a47545c4ad9a00;p=pioneer-stm32 Status response to SetVelocity messages + improved UART error code --- diff --git a/pioneer_controller/Core/Inc/firmware/fmw_messages.h b/pioneer_controller/Core/Inc/firmware/fmw_messages.h index 74f8172..1f97cfa 100644 --- a/pioneer_controller/Core/Inc/firmware/fmw_messages.h +++ b/pioneer_controller/Core/Inc/firmware/fmw_messages.h @@ -19,9 +19,7 @@ typedef union { 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; @@ -89,17 +87,17 @@ typedef struct { uint32_t update_period; } config_led; - FMW_Result response; - struct { - FMW_Result result; - uint16_t delta_millis; - int32_t ticks_left; - int32_t ticks_right; - } status_response; struct { float linear; float angular; - } velocity; + } run_set_velocity; + + struct { + int32_t ticks_left; + int32_t ticks_right; + uint16_t delta_millis; + FMW_Result result; + } response; }; } FMW_Message; #pragma pack(pop) diff --git a/pioneer_controller/Core/Src/firmware/fwm_core.c b/pioneer_controller/Core/Src/firmware/fwm_core.c index 68f6499..4e5a74f 100644 --- a/pioneer_controller/Core/Src/firmware/fwm_core.c +++ b/pioneer_controller/Core/Src/firmware/fwm_core.c @@ -38,19 +38,19 @@ FMW_Message fmw_message_from_uart_error(const UART_HandleTypeDef *huart) { res.header.type = FMW_MessageType_Response; switch (huart->ErrorCode) { case HAL_UART_ERROR_PE: { - res.response = FMW_Result_Error_UART_Parity; + res.response.result = FMW_Result_Error_UART_Parity; } break; case HAL_UART_ERROR_FE: { - res.response = FMW_Result_Error_UART_Frame; + res.response.result = FMW_Result_Error_UART_Frame; } break; case HAL_UART_ERROR_NE: { - res.response = FMW_Result_Error_UART_Noise; + res.response.result = FMW_Result_Error_UART_Noise; } break; case HAL_UART_ERROR_ORE: { - res.response = FMW_Result_Error_UART_Overrun; + res.response.result = FMW_Result_Error_UART_Overrun; } break; case HAL_UART_ERROR_RTO: { - res.response = FMW_Result_Error_UART_ReceiveTimeoutElapsed; + res.response.result = FMW_Result_Error_UART_ReceiveTimeoutElapsed; } break; default: { // NOTE(lb): unreachable FMW_ASSERT(false, .callback = fmw_hook_assert_fail); diff --git a/pioneer_controller/Core/Src/main.c b/pioneer_controller/Core/Src/main.c index db12543..0498071 100644 --- a/pioneer_controller/Core/Src/main.c +++ b/pioneer_controller/Core/Src/main.c @@ -33,7 +33,7 @@ /* Private typedef -----------------------------------------------------------*/ /* USER CODE BEGIN PTD */ FMW_Result message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) - __attribute__((warn_unused_result, nonnull)); + __attribute__((nonnull)); /* USER CODE END PTD */ /* Private define ------------------------------------------------------------*/ @@ -842,6 +842,7 @@ FMW_Result message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) { if (!(crc_computed == crc_received)) { return FMW_Result_Error_UART_Crc; } } + FMW_Result result = FMW_Result_Ok; switch (current_mode) { case FMW_Mode_Init: { switch (msg->header.type) { @@ -850,15 +851,18 @@ FMW_Result message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) { } break; case FMW_MessageType_Config_Robot: { if (!(msg->config_robot.baseline > 0.f)) { - return FMW_Result_Error_MessageHandler_Init_NonPositiveBaseline; + result = FMW_Result_Error_MessageHandler_Init_NonPositiveBaseline; + goto msg_contains_error; } if (!(msg->config_robot.wheel_circumference_left > 0.f && msg->config_robot.wheel_circumference_right > 0.f)) { - return FMW_Result_Error_MessageHandler_Init_NonPositiveWheelCircumference; + result = FMW_Result_Error_MessageHandler_Init_NonPositiveWheelCircumference; + goto msg_contains_error; } if (!(msg->config_robot.ticks_per_revolution_left > 0 && msg->config_robot.ticks_per_revolution_right > 0)) { - return FMW_Result_Error_MessageHandler_Init_NonPositiveTicksPerRevolution; + result = FMW_Result_Error_MessageHandler_Init_NonPositiveTicksPerRevolution; + goto msg_contains_error; } odometry.baseline = msg->config_robot.baseline; @@ -874,7 +878,8 @@ FMW_Result message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) { } break; case FMW_MessageType_Config_LED: { if (!(msg->config_led.update_period > 0)) { - return FMW_Result_Error_MessageHandler_Init_NonPositiveLEDUpdatePeriod; + result = FMW_Result_Error_MessageHandler_Init_NonPositiveLEDUpdatePeriod; + goto msg_contains_error; } pled.voltage_red = msg->config_led.voltage_red; @@ -884,15 +889,23 @@ FMW_Result message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) { } break; case FMW_MessageType_Run_GetStatus: // NOTE(lb): allow status messages in init mode? case FMW_MessageType_Run_SetVelocity: { - return FMW_Result_Error_Command_NotAvailable; + result = FMW_Result_Error_Command_NotAvailable; + goto msg_contains_error; } break; default: { - return FMW_Result_Error_Command_NotRecognized; + result = FMW_Result_Error_Command_NotRecognized; + goto msg_contains_error; } break; } } break; case FMW_Mode_Run: { switch (msg->header.type) { + case FMW_MessageType_Run_SetVelocity: { + fmw_odometry_setpoint_from_velocities(&odometry, msg->run_set_velocity.linear, msg->run_set_velocity.angular); + pid_left.setpoint = odometry.setpoint_left; + pid_right.setpoint = odometry.setpoint_right; + pid_cross.setpoint = odometry.setpoint_left - odometry.setpoint_right; + } // fallthrough case FMW_MessageType_Run_GetStatus: { int32_t current_ticks_left = ticks_left + fmw_encoder_count_get(&encoders.left); int32_t current_ticks_right = ticks_right + fmw_encoder_count_get(&encoders.right); @@ -904,34 +917,42 @@ FMW_Result message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) { time_millis_previous = time_millis_current; FMW_Message msg = {0}; - msg.header.type = FMW_MessageType_Run_GetStatus_Response; - msg.status_response.delta_millis = time_millis_delta; - msg.status_response.ticks_left = current_ticks_left; - msg.status_response.ticks_right = current_ticks_right; + msg.header.type = FMW_MessageType_Response; + msg.response.result = FMW_Result_Ok; + msg.response.delta_millis = time_millis_delta; + msg.response.ticks_left = current_ticks_left; + msg.response.ticks_right = current_ticks_right; msg.header.crc = HAL_CRC_Calculate(hcrc, (uint32_t*)&msg, sizeof msg); - (void)HAL_UART_Transmit(UART_MESSANGER_HANDLE, (uint8_t*)&msg, sizeof msg, HAL_MAX_DELAY); + return FMW_Result_Ok; } break; - case FMW_MessageType_Run_SetVelocity: { - fmw_odometry_setpoint_from_velocities(&odometry, msg->velocity.linear, msg->velocity.angular); - pid_left.setpoint = odometry.setpoint_left; - pid_right.setpoint = odometry.setpoint_right; - pid_cross.setpoint = odometry.setpoint_left - odometry.setpoint_right; + case FMW_MessageType_StateChange_Init: { + fmw_encoder_count_reset(&encoders.left); + fmw_encoder_count_reset(&encoders.right); + fmw_motor_brake(motors.values, ARRLENGTH(motors.values)); + current_mode = FMW_Mode_Init; } break; case FMW_MessageType_StateChange_Run: case FMW_MessageType_Config_Robot: case FMW_MessageType_Config_PID: case FMW_MessageType_Config_LED: { - return FMW_Result_Error_Command_NotAvailable; + result = FMW_Result_Error_Command_NotAvailable; + goto msg_contains_error; } break; default: { - return FMW_Result_Error_Command_NotRecognized; + result = FMW_Result_Error_Command_NotRecognized; + goto msg_contains_error; } break; } } break; } - return FMW_Result_Ok; + msg_contains_error:; + FMW_Message response = {0}; + response.header.type = FMW_MessageType_Response; + response.response.result = result; + fmw_message_uart_send(UART_MESSANGER_HANDLE, hcrc, &response, 1); + return result; } // TIMER 100Hz PID control @@ -962,21 +983,24 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) { 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); + (void)message_handler((FMW_Message*)&uart_message_buffer, &hcrc); // 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) { 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); + if (huart->RxState == HAL_UART_STATE_BUSY_RX) { + 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); + } else { + fmw_motor_brake(motors.values, ARRLENGTH(motors.values)); + FMW_ASSERT(huart->gState == HAL_UART_STATE_BUSY_TX); + FMW_ASSERT(false); + } } void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { diff --git a/pioneer_controller/pioneer_controller.ioc b/pioneer_controller/pioneer_controller.ioc index 175b7bc..ab3cea3 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,0-MX_CORTEX_M7_Init-CORTEX_M7-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,11-MX_CRC_Init-CRC-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 5ff24c8..bc7e08e 100644 --- a/pioneer_workstation/src/main.c +++ b/pioneer_workstation/src/main.c @@ -59,7 +59,10 @@ static void pprint_message(const FMW_Message *msg) { msg->header.crc); switch (msg->header.type) { case FMW_MessageType_Response: { - printf("\n response: %s", result_types[msg->response]); + printf("\n response: %s", result_types[msg->response.result]); + printf("\n delta_millis: %d", msg->response.delta_millis); + printf("\n ticks_left: %d", msg->response.ticks_left); + printf("\n ticks_right: %d", msg->response.ticks_right); } break; default: { assert(0 && "unreachable"); @@ -93,8 +96,9 @@ int main(void) { if (n >= 0) { bytes_read += (uint32_t)n; } } assert(response.header.type == FMW_MessageType_Response); + printf("\tconfig:robot\n"); pprint_message(&response); - assert(response.response == FMW_Result_Ok); + assert(response.response.result == FMW_Result_Ok); FMW_Message msg_config_pid = { .header = { @@ -125,8 +129,9 @@ int main(void) { if (n >= 0) { bytes_read += (uint32_t)n; } } assert(response.header.type == FMW_MessageType_Response); + printf("\tconfig:pid\n"); pprint_message(&response); - assert(response.response == FMW_Result_Ok); + assert(response.response.result == FMW_Result_Ok); FMW_Message msg_run = { .header = { @@ -140,8 +145,60 @@ int main(void) { if (n >= 0) { bytes_read += (uint32_t)n; } } assert(response.header.type == FMW_MessageType_Response); + printf("\trun\n"); + pprint_message(&response); + assert(response.response.result == FMW_Result_Ok); + + FMW_Message msg_get_status = { + .header = { + .type = FMW_MessageType_Run_GetStatus, + .crc = (uint32_t)-1, + }, + }; + write(fd, &msg_get_status, sizeof(FMW_Message)); + for (uint32_t bytes_read = 0; bytes_read < sizeof(response);) { + ssize_t n = read(fd, ((uint8_t*)&response) + bytes_read, sizeof(response) - bytes_read); + if (n >= 0) { bytes_read += (uint32_t)n; } + } + assert(response.header.type == FMW_MessageType_Response); + printf("\trun:get_status\n"); + pprint_message(&response); + + for (int i = 0; i < 100; ++i) { + FMW_Message msg_set_speed = { + .header = { + .type = FMW_MessageType_Run_SetVelocity, + .crc = (uint32_t)-1, + }, + .run_set_velocity = { + .linear = 10.f, + .angular = 2.5f, + }, + }; + write(fd, &msg_set_speed, sizeof(FMW_Message)); + for (uint32_t bytes_read = 0; bytes_read < sizeof(response);) { + ssize_t n = read(fd, ((uint8_t*)&response) + bytes_read, sizeof(response) - bytes_read); + if (n >= 0) { bytes_read += (uint32_t)n; } + } + assert(response.header.type == FMW_MessageType_Response); + printf("\trun:set_speed\n"); + pprint_message(&response); + } + + FMW_Message msg_init = { + .header = { + .type = FMW_MessageType_StateChange_Init, + .crc = (uint32_t)-1, + }, + }; + write(fd, &msg_init, sizeof(FMW_Message)); + for (uint32_t bytes_read = 0; bytes_read < sizeof(response);) { + ssize_t n = read(fd, ((uint8_t*)&response) + bytes_read, sizeof(response) - bytes_read); + if (n >= 0) { bytes_read += (uint32_t)n; } + } + assert(response.header.type == FMW_MessageType_Response); + printf("\tinit\n"); pprint_message(&response); - assert(response.response == FMW_Result_Ok); close(fd); }