From: LeonardoBizzoni Date: Thu, 12 Mar 2026 17:03:33 +0000 (+0100) Subject: improved UART error callback + renamed fn with arrays as input X-Git-Url: http://git.leonardobizzoni.com/?a=commitdiff_plain;h=943acc4b3e567776fd6584975b7d45ca97413a7e;p=pioneer-stm32 improved UART error callback + renamed fn with arrays as input --- diff --git a/pioneer_controller/Core/Inc/firmware/fmw_core.h b/pioneer_controller/Core/Inc/firmware/fmw_core.h index 93246fb..16baa05 100644 --- a/pioneer_controller/Core/Inc/firmware/fmw_core.h +++ b/pioneer_controller/Core/Inc/firmware/fmw_core.h @@ -79,13 +79,15 @@ typedef struct FMW_Hook { void *args; } FMW_Hook; -void fmw_motor_init(FMW_Motor motors[], int32_t count) __attribute__((nonnull)); +void fmw_motors_init(FMW_Motor motors[], int32_t count) __attribute__((nonnull)); +void fmw_motors_deinit(FMW_Motor motors[], int32_t count) __attribute__((nonnull)); +void fmw_motors_stop(FMW_Motor motors[], int32_t count) __attribute__((nonnull)); +void fmw_motors_enable(FMW_Motor motors[], int32_t count) __attribute__((nonnull)); +void fmw_motors_disable(FMW_Motor motors[], int32_t count) __attribute__((nonnull)); void fmw_motor_set_speed(FMW_Motor *motor, int32_t duty_cycle) __attribute__((nonnull)); -void fmw_motor_brake(FMW_Motor motors[], int32_t count) __attribute__((nonnull)); -void fmw_motor_enable(FMW_Motor motors[], int32_t count) __attribute__((nonnull)); -void fmw_motor_disable(FMW_Motor motors[], int32_t count) __attribute__((nonnull)); -void fmw_encoder_init(FMW_Encoder encoders[], int32_t count) __attribute__((nonnull)); +void fmw_encoders_init(FMW_Encoder encoders[], int32_t count) __attribute__((nonnull)); +void fmw_encoders_deinit(FMW_Encoder encoders[], int32_t count) __attribute__((nonnull)); void fmw_encoder_update(FMW_Encoder *encoder) __attribute__((nonnull)); float fmw_encoder_get_linear_velocity(const FMW_Encoder *encoder) __attribute__((warn_unused_result, nonnull)); void fmw_encoder_count_reset(FMW_Encoder *encoder) __attribute__((nonnull)); @@ -96,12 +98,14 @@ int32_t fmw_pid_update(FMW_PidController *pid, float velocity) __attribute__((wa void fmw_odometry_setpoint_from_velocities(FMW_Odometry *odometry, float linear, float angular) __attribute__((nonnull)); void fmw_led_init(FMW_Led *led) __attribute__((nonnull)); +void fmw_led_deinit(FMW_Led *led) __attribute__((nonnull)); void fmw_led_update(FMW_Led *led) __attribute__((nonnull)); -void fmw_buzzer_set(FMW_Buzzer buzzer[], int32_t count, bool on) __attribute__((nonnull)); +void fmw_buzzers_set(FMW_Buzzer buzzer[], int32_t count, bool on) __attribute__((nonnull)); -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_Result fmw_message_uart_receive(UART_HandleTypeDef *huart, FMW_Message *msg, int32_t wait_ms) __attribute__((warn_unused_result, nonnull)); +HAL_StatusTypeDef fmw_message_uart_send(UART_HandleTypeDef *huart, CRC_HandleTypeDef *hcrc, + FMW_Message *msg, int32_t wait_ms) __attribute__((nonnull, warn_unused_result)); FMW_Message fmw_message_from_uart_error(const UART_HandleTypeDef *huart); diff --git a/pioneer_controller/Core/Inc/firmware/fmw_messages.h b/pioneer_controller/Core/Inc/firmware/fmw_messages.h index 0d40b14..2a833d8 100644 --- a/pioneer_controller/Core/Inc/firmware/fmw_messages.h +++ b/pioneer_controller/Core/Inc/firmware/fmw_messages.h @@ -13,8 +13,8 @@ typedef union { #define FMW_MESSAGE_TYPE_VARIANTS(X) \ X(FMW_MessageType_None) \ X(FMW_MessageType_Response) \ - X(FMW_MessageType_StateChange_Config) \ - X(FMW_MessageType_StateChange_Run) \ + X(FMW_MessageType_ModeChange_Config) \ + X(FMW_MessageType_ModeChange_Run) \ X(FMW_MessageType_Config_Robot) \ X(FMW_MessageType_Config_PID) \ X(FMW_MessageType_Config_LED) \ @@ -32,6 +32,7 @@ enum { #define FMW_RESULT_VARIANTS(X) \ X(FMW_Result_Ok) \ X(FMW_Result_Error_InvalidArguments) \ + X(FMW_Result_Error_FaultPinTriggered) \ X(FMW_Result_Error_UART_Crc) \ X(FMW_Result_Error_UART_NegativeTimeout) \ X(FMW_Result_Error_UART_ReceiveTimeoutElapsed) \ @@ -61,7 +62,7 @@ enum { }; #pragma pack(push, 1) -typedef struct { +typedef struct FMW_Message { struct { FMW_MessageType type; uint32_t crc; @@ -102,4 +103,9 @@ typedef struct { } FMW_Message; #pragma pack(pop) +static_assert(sizeof(uint8_t) == 1); +static_assert(sizeof(uint16_t) == 2); +static_assert(sizeof(uint32_t) == 4); +static_assert(sizeof(float) == 4); + #endif /* INC_COMMUNICATION_OTTO_MESSAGES_H_ */ diff --git a/pioneer_controller/Core/Src/firmware/fwm_core.c b/pioneer_controller/Core/Src/firmware/fwm_core.c index 4e5a74f..58d7a2c 100644 --- a/pioneer_controller/Core/Src/firmware/fwm_core.c +++ b/pioneer_controller/Core/Src/firmware/fwm_core.c @@ -10,7 +10,7 @@ static struct { static void fmw_hook_assert_fail(void *_) { if (fmw_state.motors != NULL) { - fmw_motor_brake(fmw_state.motors, fmw_state.motors_count); + fmw_motors_stop(fmw_state.motors, fmw_state.motors_count); } } @@ -26,10 +26,10 @@ FMW_Result fmw_message_uart_receive(UART_HandleTypeDef *huart, FMW_Message *msg, return FMW_Result_Ok; } -void fmw_message_uart_send(UART_HandleTypeDef *huart, CRC_HandleTypeDef *hcrc, FMW_Message *msg, int32_t wait_ms) { +HAL_StatusTypeDef fmw_message_uart_send(UART_HandleTypeDef *huart, CRC_HandleTypeDef *hcrc, FMW_Message *msg, int32_t wait_ms) { msg->header.crc = HAL_CRC_Calculate(hcrc, (uint32_t*)msg, sizeof *msg); HAL_StatusTypeDef res = HAL_UART_Transmit(huart, (uint8_t*)msg, sizeof *msg, wait_ms); - FMW_ASSERT(res == HAL_OK, .callback = fmw_hook_assert_fail); + return res; } FMW_Message fmw_message_from_uart_error(const UART_HandleTypeDef *huart) { @@ -61,15 +61,15 @@ FMW_Message fmw_message_from_uart_error(const UART_HandleTypeDef *huart) { // ============================================================ // Motor controller -void fmw_motor_init(FMW_Motor motors[], int32_t count) { +void fmw_motors_init(FMW_Motor motors[], int32_t count) { FMW_ASSERT(count > 0); fmw_state.motors = motors; fmw_state.motors_count = count; for (int32_t i = 0; i < count; ++i) { - FMW_ASSERT(motors[i].sleep_gpio_port); - FMW_ASSERT(motors[i].dir_gpio_port); - FMW_ASSERT(motors[i].pwm_timer); + FMW_ASSERT(motors[i].sleep_gpio_port != NULL); + FMW_ASSERT(motors[i].dir_gpio_port != NULL); + FMW_ASSERT(motors[i].pwm_timer != NULL); FMW_ASSERT(motors[i].pwm_channel == TIM_CHANNEL_1 || motors[i].pwm_channel == TIM_CHANNEL_2 || motors[i].pwm_channel == TIM_CHANNEL_3 || motors[i].pwm_channel == TIM_CHANNEL_4 || motors[i].pwm_channel == TIM_CHANNEL_5 || motors[i].pwm_channel == TIM_CHANNEL_6 || @@ -97,7 +97,15 @@ void fmw_motor_init(FMW_Motor motors[], int32_t count) { motors[i].max_dutycycle = motors[i].pwm_timer->Instance->ARR; motors[i].active = true; } - fmw_motor_brake(motors, count); + fmw_motors_stop(motors, count); +} + +void fmw_motors_deinit(FMW_Motor motors[], int32_t count) { + for (int32_t i = 0; i < count; ++i) { + HAL_StatusTypeDef status = HAL_TIM_PWM_Stop(motors[i].pwm_timer, motors[i].pwm_channel); + FMW_ASSERT(status == HAL_OK); + motors[i].active = false; + } } void fmw_motor_set_speed(FMW_Motor *motor, int32_t duty_cycle) { @@ -118,7 +126,7 @@ void fmw_motor_set_speed(FMW_Motor *motor, int32_t duty_cycle) { HAL_GPIO_WritePin(motor->sleep_gpio_port, motor->sleep_pin, GPIO_PIN_SET); } -void fmw_motor_brake(FMW_Motor motors[], int32_t count) { +void fmw_motors_stop(FMW_Motor motors[], int32_t count) { FMW_ASSERT(count > 0); for (int32_t i = 0; i < count; ++i) { FMW_ASSERT(motors[i].sleep_gpio_port != NULL); @@ -136,12 +144,12 @@ void fmw_motor_brake(FMW_Motor motors[], int32_t count) { motors[i].pwm_channel == TIM_CHANNEL_5 || motors[i].pwm_channel == TIM_CHANNEL_6 || motors[i].pwm_channel == TIM_CHANNEL_ALL); - HAL_GPIO_WritePin(motors[i].sleep_gpio_port, motors[i].sleep_pin, FMW_MotorDirection_Backward); + HAL_GPIO_WritePin(motors[i].sleep_gpio_port, motors[i].sleep_pin, GPIO_PIN_RESET); __HAL_TIM_SET_COMPARE(motors[i].pwm_timer, motors[i].pwm_channel, 0); } } -void fmw_motor_enable(FMW_Motor motors[], int32_t count) { +void fmw_motors_enable(FMW_Motor motors[], int32_t count) { FMW_ASSERT(count > 0); for (int32_t i = 0; i < count; ++i) { FMW_ASSERT(motors[i].pwm_timer != NULL); @@ -156,7 +164,7 @@ void fmw_motor_enable(FMW_Motor motors[], int32_t count) { } } -void fmw_motor_disable(FMW_Motor motors[], int32_t count) { +void fmw_motors_disable(FMW_Motor motors[], int32_t count) { FMW_ASSERT(count > 0, .callback = fmw_hook_assert_fail); for (int32_t i = 0; i < count; ++i) { FMW_ASSERT(motors[i].pwm_timer != NULL, .callback = fmw_hook_assert_fail); @@ -173,7 +181,7 @@ void fmw_motor_disable(FMW_Motor motors[], int32_t count) { // ============================================================ // Encoder -void fmw_encoder_init(FMW_Encoder encoders[], int32_t count) { +void fmw_encoders_init(FMW_Encoder encoders[], int32_t count) { for (int32_t i = 0; i < count; ++i) { FMW_ASSERT(encoders[i].timer != NULL); FMW_ASSERT(encoders[i].ticks_per_revolution > 0); @@ -190,6 +198,14 @@ void fmw_encoder_init(FMW_Encoder encoders[], int32_t count) { } } +void fmw_encoders_deinit(FMW_Encoder encoders[], int32_t count) { + for (int32_t i = 0; i < count; ++i) { + FMW_ASSERT(encoders[i].timer != NULL); + HAL_StatusTypeDef status = HAL_TIM_Encoder_Stop(encoders[i].timer, TIM_CHANNEL_ALL); + FMW_ASSERT(status == HAL_OK); + } +} + void fmw_encoder_update(FMW_Encoder *encoder) { encoder->previous_millis = encoder->current_millis; encoder->current_millis = HAL_GetTick(); @@ -211,12 +227,12 @@ float fmw_encoder_get_linear_velocity(const FMW_Encoder *encoder) { } void fmw_encoder_count_reset(FMW_Encoder *encoder) { - FMW_ASSERT(encoder->timer, .callback = fmw_hook_assert_fail); + FMW_ASSERT(encoder->timer != NULL, .callback = fmw_hook_assert_fail); __HAL_TIM_SET_COUNTER(encoder->timer, (encoder->timer->Init.Period / 2)); } int32_t fmw_encoder_count_get(const FMW_Encoder *encoder) { - FMW_ASSERT(encoder->timer, .callback = fmw_hook_assert_fail); + FMW_ASSERT(encoder->timer != NULL, .callback = fmw_hook_assert_fail); return (int32_t)__HAL_TIM_GET_COUNTER(encoder->timer) - (encoder->timer->Init.Period / 2); } @@ -250,8 +266,8 @@ int32_t fmw_pid_update(FMW_PidController *pid, float velocity) { // ============================================================ // LEDs void fmw_led_init(FMW_Led *led) { - FMW_ASSERT(led->timer); - FMW_ASSERT(led->adc); + FMW_ASSERT(led->timer != NULL); + FMW_ASSERT(led->adc != NULL); FMW_ASSERT(led->timer_channel == TIM_CHANNEL_1 || led->timer_channel == TIM_CHANNEL_2 || led->timer_channel == TIM_CHANNEL_3 || led->timer_channel == TIM_CHANNEL_4 || led->timer_channel == TIM_CHANNEL_5 || led->timer_channel == TIM_CHANNEL_6 || @@ -266,8 +282,14 @@ void fmw_led_init(FMW_Led *led) { __HAL_TIM_SET_COMPARE(led->timer, led->timer_channel, 0); } +void fmw_led_deinit(FMW_Led *led) { + HAL_StatusTypeDef status = HAL_TIM_PWM_Stop(led->timer, led->timer_channel); + FMW_ASSERT(status == HAL_OK); +} + void fmw_led_update(FMW_Led *led) { - FMW_ASSERT(led->timer && led->adc, .callback = fmw_hook_assert_fail); + FMW_ASSERT(led->timer != NULL, .callback = fmw_hook_assert_fail); + FMW_ASSERT(led->adc != NULL, .callback = fmw_hook_assert_fail); FMW_ASSERT(led->timer_channel == TIM_CHANNEL_1 || led->timer_channel == TIM_CHANNEL_2 || led->timer_channel == TIM_CHANNEL_3 || led->timer_channel == TIM_CHANNEL_4 || led->timer_channel == TIM_CHANNEL_5 || led->timer_channel == TIM_CHANNEL_6 || @@ -332,7 +354,7 @@ void fmw_led_update(FMW_Led *led) { // ============================================================ // Buzzers // NOTE(lb): replace bool with uint8_t bitmask? -void fmw_buzzer_set(FMW_Buzzer buzzer[], int32_t count, bool on) { +void fmw_buzzers_set(FMW_Buzzer buzzer[], int32_t count, bool on) { FMW_ASSERT(count >= 0, .callback = fmw_hook_assert_fail); for (int32_t i = 0; i < count; ++i) { HAL_StatusTypeDef res; diff --git a/pioneer_controller/Core/Src/main.c b/pioneer_controller/Core/Src/main.c index e23c628..b901b77 100644 --- a/pioneer_controller/Core/Src/main.c +++ b/pioneer_controller/Core/Src/main.c @@ -38,8 +38,6 @@ FMW_Result message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) /* Private define ------------------------------------------------------------*/ /* USER CODE BEGIN PD */ -#define MOTOR_COUNT 2 -#define ENCODER_COUNT 2 #define UART_MESSANGER_HANDLE (&huart3) /* USER CODE END PD */ @@ -65,7 +63,7 @@ DMA_HandleTypeDef hdma_usart3_rx; // TODO(lb): fill with sensible default static union { - FMW_Encoder values[ENCODER_COUNT]; + FMW_Encoder values[2]; struct { FMW_Encoder right; FMW_Encoder left; @@ -116,7 +114,7 @@ FMW_PidController pid_cross = { }; static union { - FMW_Motor values[MOTOR_COUNT]; + FMW_Motor values[2]; struct { FMW_Motor right; FMW_Motor left; @@ -800,20 +798,6 @@ static void MX_GPIO_Init(void) void start(void) { // Enables UART RX interrupt HAL_UART_Receive_DMA(UART_MESSANGER_HANDLE, (uint8_t*)&uart_message_buffer, sizeof uart_message_buffer); - for (; current_mode == FMW_Mode_Config; ); - - fmw_encoder_init(encoders.values, ENCODER_COUNT); - fmw_motor_init(motors.values, MOTOR_COUNT); - fmw_led_init(&pled); - - // Right and left motors have the same parameters - pid_max = (int32_t)htim4.Instance->ARR; - pid_min = -pid_max; - FMW_ASSERT(pid_max > pid_min); - - // Enables TIM6 interrupt (used for PID control) - HAL_StatusTypeDef timer_status = HAL_TIM_Base_Start_IT(&htim6); - FMW_ASSERT(timer_status == HAL_OK); for (;;) { switch (current_mode) { @@ -846,7 +830,20 @@ FMW_Result message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) { switch (current_mode) { case FMW_Mode_Config: { switch (msg->header.type) { - case FMW_MessageType_StateChange_Run: { + case FMW_MessageType_ModeChange_Run: { + fmw_encoders_init(encoders.values, ARRLENGTH(encoders.values)); + fmw_motors_init(motors.values, ARRLENGTH(motors.values)); + fmw_led_init(&pled); + + // Right and left motors have the same parameters + pid_max = (int32_t)htim4.Instance->ARR; + pid_min = -pid_max; + FMW_ASSERT(pid_max > pid_min); + + // Enables TIM6 interrupt (used for PID control) + HAL_StatusTypeDef timer_status = HAL_TIM_Base_Start_IT(&htim6); + FMW_ASSERT(timer_status == HAL_OK); + current_mode = FMW_Mode_Run; } break; case FMW_MessageType_Config_Robot: { @@ -887,7 +884,7 @@ FMW_Result message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) { pled.voltage_hysteresis = msg->config_led.voltage_hysteresis; led_update_period = msg->config_led.update_period; } break; - case FMW_MessageType_Run_GetStatus: // NOTE(lb): allow status messages in config mode? + case FMW_MessageType_Run_GetStatus: case FMW_MessageType_Run_SetVelocity: { result = FMW_Result_Error_Command_NotAvailable; goto msg_contains_error; @@ -926,13 +923,21 @@ FMW_Result message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) { (void)HAL_UART_Transmit(UART_MESSANGER_HANDLE, (uint8_t*)&msg, sizeof msg, HAL_MAX_DELAY); return FMW_Result_Ok; } break; - case FMW_MessageType_StateChange_Config: { + case FMW_MessageType_ModeChange_Config: { + fmw_motors_stop(motors.values, ARRLENGTH(motors.values)); fmw_encoder_count_reset(&encoders.left); fmw_encoder_count_reset(&encoders.right); - fmw_motor_brake(motors.values, ARRLENGTH(motors.values)); + + fmw_encoders_deinit(encoders.values, ARRLENGTH(encoders.values)); + fmw_motors_deinit(motors.values, ARRLENGTH(motors.values)); + fmw_led_deinit(&pled); + + HAL_StatusTypeDef timer_status = HAL_TIM_Base_Stop_IT(&htim6); + FMW_ASSERT(timer_status == HAL_OK); + current_mode = FMW_Mode_Config; } break; - case FMW_MessageType_StateChange_Run: + case FMW_MessageType_ModeChange_Run: case FMW_MessageType_Config_Robot: case FMW_MessageType_Config_PID: case FMW_MessageType_Config_LED: { @@ -951,7 +956,8 @@ FMW_Result message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) { FMW_Message response = {0}; response.header.type = FMW_MessageType_Response; response.response.result = result; - fmw_message_uart_send(UART_MESSANGER_HANDLE, hcrc, &response, 1); + HAL_StatusTypeDef send_res = fmw_message_uart_send(UART_MESSANGER_HANDLE, hcrc, &response, 1); + FMW_ASSERT(send_res == HAL_OK); return result; } @@ -991,16 +997,18 @@ void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) { void HAL_UART_ErrorCallback(UART_HandleTypeDef *huart) { if (huart != UART_MESSANGER_HANDLE) { return; } - 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); + fmw_motors_stop(motors.values, ARRLENGTH(motors.values)); + FMW_Message response = fmw_message_from_uart_error(huart); + + retry:; + HAL_StatusTypeDef res = fmw_message_uart_send(UART_MESSANGER_HANDLE, &hcrc, &response, HAL_MAX_DELAY); + if (res != HAL_OK) { // NOTE(lb): send help + fmw_buzzers_set(&buzzer, 1, true); + goto retry; } + + 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) { @@ -1011,24 +1019,33 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { if (time_now - time_last_motors > FMW_DEBOUNCE_DELAY) { time_last_motors = time_now; if (motors.left.active && motors.right.active) { - fmw_motor_disable(motors.values, ARRLENGTH(motors.values)); + fmw_motors_disable(motors.values, ARRLENGTH(motors.values)); HAL_GPIO_WritePin(SLED_GPIO_Port, SLED_Pin, GPIO_PIN_RESET); - fmw_buzzer_set(&buzzer, 1, false); + fmw_buzzers_set(&buzzer, 1, false); } else { FMW_ASSERT(!motors.left.active); FMW_ASSERT(!motors.right.active); - fmw_motor_enable(motors.values, ARRLENGTH(motors.values)); + fmw_motors_enable(motors.values, ARRLENGTH(motors.values)); HAL_GPIO_WritePin(SLED_GPIO_Port, SLED_Pin, GPIO_PIN_SET); - fmw_buzzer_set(&buzzer, 1, false); + fmw_buzzers_set(&buzzer, 1, false); } } } break; case fault1_Pin: case fault2_Pin: { - fmw_motor_brake(motors.values, ARRLENGTH(motors.values)); - // stop TIM6 interrupt (used for PID control) + fmw_motors_stop(motors.values, ARRLENGTH(motors.values)); + FMW_Message response = {0}; + response.header.type = FMW_MessageType_Response; + response.response.result = FMW_Result_Error_FaultPinTriggered; + + retry:; + HAL_StatusTypeDef res = fmw_message_uart_send(UART_MESSANGER_HANDLE, &hcrc, &response, HAL_MAX_DELAY); + if (res != HAL_OK) { // NOTE(lb): send help + fmw_buzzers_set(&buzzer, 1, true); + goto retry; + } + HAL_TIM_Base_Stop_IT(&htim6); - /* otto_status = MessageStatusCode_Fault_HBridge; */ } break; } } diff --git a/pioneer_workstation/src/main.c b/pioneer_workstation/src/main.c index 31842ac..265c89c 100644 --- a/pioneer_workstation/src/main.c +++ b/pioneer_workstation/src/main.c @@ -33,7 +33,8 @@ static int serial_open(char *portname) { tty.c_cc[VMIN] = 1; tty.c_cc[VTIME] = 0; - tcflush(fd, TCIFLUSH); + sleep(2); // NOTE(lb): required to make flush work, for some reason + tcflush(fd, TCIOFLUSH); tcsetattr(fd, TCSANOW, &tty); return fd; @@ -77,6 +78,116 @@ int main(void) { FMW_Message response = {0}; +#if 0 + FMW_Message msgs[] = { + { + .header = { + .type = FMW_MessageType_Config_Robot, + .crc = (uint32_t)-1, + }, + .config_robot = { + .baseline = 2.3f, + .wheel_circumference_left = 4.f, + .wheel_circumference_right = 3.2f, + .ticks_per_revolution_left = 250, + .ticks_per_revolution_right = 350, + }, + }, + { + .header = { + .type = FMW_MessageType_Config_PID, + .crc = (uint32_t)-1, + }, + .config_pid = { + .left = { + .proportional = 0.2f, + .integral = 0.2f, + .derivative = 0.2f, + }, + .right = { + .proportional = 0.3f, + .integral = 0.3f, + .derivative = 0.3f, + }, + .cross = { + .proportional = 0.4f, + .integral = 0.4f, + .derivative = 0.4f, + }, + }, + }, + { + .header = { + .type = FMW_MessageType_Config_Robot, + .crc = (uint32_t)-1, + }, + .config_robot = { + .baseline = 2.3f, + .wheel_circumference_left = 4.f, + .wheel_circumference_right = 3.2f, + .ticks_per_revolution_left = 250, + .ticks_per_revolution_right = 350, + }, + }, + }; + write(fd, &msgs, sizeof(msgs)); + for (int i = 0; i < 2; ++i) { + 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); + pprint_message(&response); + } +#elif 0 + FMW_Message msg_config_robot = { + .header = { + .type = FMW_MessageType_Config_Robot, + .crc = (uint32_t)-1, + }, + .config_robot = { + .baseline = 2.3f, + .wheel_circumference_left = 4.f, + .wheel_circumference_right = 3.2f, + .ticks_per_revolution_left = 250, + .ticks_per_revolution_right = 350, + }, + }; + FMW_Message msg_config_pid = { + .header = { + .type = FMW_MessageType_Config_PID, + .crc = (uint32_t)-1, + }, + .config_pid = { + .left = { + .proportional = 0.2f, + .integral = 0.2f, + .derivative = 0.2f, + }, + .right = { + .proportional = 0.3f, + .integral = 0.3f, + .derivative = 0.3f, + }, + .cross = { + .proportional = 0.4f, + .integral = 0.4f, + .derivative = 0.4f, + }, + }, + }; + + write(fd, &msg_config_robot, sizeof(FMW_Message)); + write(fd, &msg_config_pid, sizeof(FMW_Message)); + for (int i = 0; i < 2; ++i) { + 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); + pprint_message(&response); + } +#else FMW_Message msg_config_robot = { .header = { .type = FMW_MessageType_Config_Robot, @@ -95,9 +206,9 @@ int main(void) { 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("\tconfig:robot\n"); pprint_message(&response); + assert(response.header.type == FMW_MessageType_Response); assert(response.response.result == FMW_Result_Ok); FMW_Message msg_config_pid = { @@ -135,7 +246,7 @@ int main(void) { FMW_Message msg_run = { .header = { - .type = FMW_MessageType_StateChange_Run, + .type = FMW_MessageType_ModeChange_Run, .crc = (uint32_t)-1, }, }; @@ -187,7 +298,7 @@ int main(void) { FMW_Message msg_init = { .header = { - .type = FMW_MessageType_StateChange_Config, + .type = FMW_MessageType_ModeChange_Config, .crc = (uint32_t)-1, }, }; @@ -199,6 +310,7 @@ int main(void) { assert(response.header.type == FMW_MessageType_Response); printf("\tinit\n"); pprint_message(&response); +#endif close(fd); }