/* Private typedef -----------------------------------------------------------*/\r
/* USER CODE BEGIN PTD */\r
FMW_Result message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc)\r
- __attribute__((warn_unused_result, nonnull));\r
+ __attribute__((nonnull));\r
/* USER CODE END PTD */\r
\r
/* Private define ------------------------------------------------------------*/\r
if (!(crc_computed == crc_received)) { return FMW_Result_Error_UART_Crc; }\r
}\r
\r
+ FMW_Result result = FMW_Result_Ok;\r
switch (current_mode) {\r
case FMW_Mode_Init: {\r
switch (msg->header.type) {\r
} break;\r
case FMW_MessageType_Config_Robot: {\r
if (!(msg->config_robot.baseline > 0.f)) {\r
- return FMW_Result_Error_MessageHandler_Init_NonPositiveBaseline;\r
+ result = FMW_Result_Error_MessageHandler_Init_NonPositiveBaseline;\r
+ goto msg_contains_error;\r
}\r
if (!(msg->config_robot.wheel_circumference_left > 0.f &&\r
msg->config_robot.wheel_circumference_right > 0.f)) {\r
- return FMW_Result_Error_MessageHandler_Init_NonPositiveWheelCircumference;\r
+ result = FMW_Result_Error_MessageHandler_Init_NonPositiveWheelCircumference;\r
+ goto msg_contains_error;\r
}\r
if (!(msg->config_robot.ticks_per_revolution_left > 0 &&\r
msg->config_robot.ticks_per_revolution_right > 0)) {\r
- return FMW_Result_Error_MessageHandler_Init_NonPositiveTicksPerRevolution;\r
+ result = FMW_Result_Error_MessageHandler_Init_NonPositiveTicksPerRevolution;\r
+ goto msg_contains_error;\r
}\r
\r
odometry.baseline = msg->config_robot.baseline;\r
} break;\r
case FMW_MessageType_Config_LED: {\r
if (!(msg->config_led.update_period > 0)) {\r
- return FMW_Result_Error_MessageHandler_Init_NonPositiveLEDUpdatePeriod;\r
+ result = FMW_Result_Error_MessageHandler_Init_NonPositiveLEDUpdatePeriod;\r
+ goto msg_contains_error;\r
}\r
\r
pled.voltage_red = msg->config_led.voltage_red;\r
} break;\r
case FMW_MessageType_Run_GetStatus: // NOTE(lb): allow status messages in init mode?\r
case FMW_MessageType_Run_SetVelocity: {\r
- return FMW_Result_Error_Command_NotAvailable;\r
+ result = FMW_Result_Error_Command_NotAvailable;\r
+ goto msg_contains_error;\r
} break;\r
default: {\r
- return FMW_Result_Error_Command_NotRecognized;\r
+ result = FMW_Result_Error_Command_NotRecognized;\r
+ goto msg_contains_error;\r
} break;\r
}\r
} break;\r
case FMW_Mode_Run: {\r
switch (msg->header.type) {\r
+ case FMW_MessageType_Run_SetVelocity: {\r
+ fmw_odometry_setpoint_from_velocities(&odometry, msg->run_set_velocity.linear, msg->run_set_velocity.angular);\r
+ pid_left.setpoint = odometry.setpoint_left;\r
+ pid_right.setpoint = odometry.setpoint_right;\r
+ pid_cross.setpoint = odometry.setpoint_left - odometry.setpoint_right;\r
+ } // fallthrough\r
case FMW_MessageType_Run_GetStatus: {\r
int32_t current_ticks_left = ticks_left + fmw_encoder_count_get(&encoders.left);\r
int32_t current_ticks_right = ticks_right + fmw_encoder_count_get(&encoders.right);\r
time_millis_previous = time_millis_current;\r
\r
FMW_Message msg = {0};\r
- msg.header.type = FMW_MessageType_Run_GetStatus_Response;\r
- msg.status_response.delta_millis = time_millis_delta;\r
- msg.status_response.ticks_left = current_ticks_left;\r
- msg.status_response.ticks_right = current_ticks_right;\r
+ msg.header.type = FMW_MessageType_Response;\r
+ msg.response.result = FMW_Result_Ok;\r
+ msg.response.delta_millis = time_millis_delta;\r
+ msg.response.ticks_left = current_ticks_left;\r
+ msg.response.ticks_right = current_ticks_right;\r
msg.header.crc = HAL_CRC_Calculate(hcrc, (uint32_t*)&msg, sizeof msg);\r
-\r
(void)HAL_UART_Transmit(UART_MESSANGER_HANDLE, (uint8_t*)&msg, sizeof msg, HAL_MAX_DELAY);\r
+ return FMW_Result_Ok;\r
} break;\r
- case FMW_MessageType_Run_SetVelocity: {\r
- fmw_odometry_setpoint_from_velocities(&odometry, msg->velocity.linear, msg->velocity.angular);\r
- pid_left.setpoint = odometry.setpoint_left;\r
- pid_right.setpoint = odometry.setpoint_right;\r
- pid_cross.setpoint = odometry.setpoint_left - odometry.setpoint_right;\r
+ case FMW_MessageType_StateChange_Init: {\r
+ fmw_encoder_count_reset(&encoders.left);\r
+ fmw_encoder_count_reset(&encoders.right);\r
+ fmw_motor_brake(motors.values, ARRLENGTH(motors.values));\r
+ current_mode = FMW_Mode_Init;\r
} break;\r
case FMW_MessageType_StateChange_Run:\r
case FMW_MessageType_Config_Robot:\r
case FMW_MessageType_Config_PID:\r
case FMW_MessageType_Config_LED: {\r
- return FMW_Result_Error_Command_NotAvailable;\r
+ result = FMW_Result_Error_Command_NotAvailable;\r
+ goto msg_contains_error;\r
} break;\r
default: {\r
- return FMW_Result_Error_Command_NotRecognized;\r
+ result = FMW_Result_Error_Command_NotRecognized;\r
+ goto msg_contains_error;\r
} break;\r
}\r
} break;\r
}\r
\r
- return FMW_Result_Ok;\r
+ msg_contains_error:;\r
+ FMW_Message response = {0};\r
+ response.header.type = FMW_MessageType_Response;\r
+ response.response.result = result;\r
+ fmw_message_uart_send(UART_MESSANGER_HANDLE, hcrc, &response, 1);\r
+ return result;\r
}\r
\r
// TIMER 100Hz PID control\r
\r
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) {\r
if (huart != UART_MESSANGER_HANDLE) { return; }\r
- FMW_Message response = {0};\r
- response.header.type = FMW_MessageType_Response;\r
- response.response = message_handler((FMW_Message*)&uart_message_buffer, &hcrc);\r
- fmw_message_uart_send(UART_MESSANGER_HANDLE, &hcrc, &response, HAL_MAX_DELAY);\r
+ (void)message_handler((FMW_Message*)&uart_message_buffer, &hcrc);\r
// NOTE(lb): listen for the next message.\r
HAL_UART_Receive_DMA(UART_MESSANGER_HANDLE, (uint8_t*)&uart_message_buffer, sizeof uart_message_buffer);\r
}\r
\r
void HAL_UART_ErrorCallback(UART_HandleTypeDef *huart) {\r
if (huart != UART_MESSANGER_HANDLE) { return; }\r
- FMW_Message response = fmw_message_from_uart_error(huart);\r
- fmw_message_uart_send(UART_MESSANGER_HANDLE, &hcrc, &response, HAL_MAX_DELAY);\r
\r
- HAL_UART_AbortReceive(huart);\r
- HAL_UART_Receive_DMA(UART_MESSANGER_HANDLE, (uint8_t*)&uart_message_buffer, sizeof uart_message_buffer);\r
+ if (huart->RxState == HAL_UART_STATE_BUSY_RX) {\r
+ FMW_Message response = fmw_message_from_uart_error(huart);\r
+ fmw_message_uart_send(UART_MESSANGER_HANDLE, &hcrc, &response, HAL_MAX_DELAY);\r
+ HAL_UART_AbortReceive(huart);\r
+ HAL_UART_Receive_DMA(UART_MESSANGER_HANDLE, (uint8_t*)&uart_message_buffer, sizeof uart_message_buffer);\r
+ } else {\r
+ fmw_motor_brake(motors.values, ARRLENGTH(motors.values));\r
+ FMW_ASSERT(huart->gState == HAL_UART_STATE_BUSY_TX);\r
+ FMW_ASSERT(false);\r
+ }\r
}\r
\r
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {\r
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");
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 = {
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 = {
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);
}
projects / pioneer-stm32 / commitdiff