From: LeonardoBizzoni Date: Tue, 10 Feb 2026 13:11:21 +0000 (+0100) Subject: custom assert with optional on-trap callback X-Git-Url: http://git.leonardobizzoni.com/?a=commitdiff_plain;h=ed45fc705d252579f48f44dee8ca52e2d0441fc8;p=pioneer-stm32 custom assert with optional on-trap callback --- diff --git a/pioneer_controller/Core/Inc/firmware/fmw_core.h b/pioneer_controller/Core/Inc/firmware/fmw_core.h index 5f1ac82..3001749 100644 --- a/pioneer_controller/Core/Inc/firmware/fmw_core.h +++ b/pioneer_controller/Core/Inc/firmware/fmw_core.h @@ -1,7 +1,7 @@ #ifndef FMW_CORE_H #define FMW_CORE_H -typedef struct { +typedef struct FMW_Encoder { TIM_HandleTypeDef *const timer; uint32_t previous_millis; uint32_t current_millis; @@ -10,7 +10,7 @@ typedef struct { float wheel_circumference; // NOTE(lb): measured in meters } FMW_Encoder; -typedef struct { +typedef struct FMW_Motor { GPIO_TypeDef *const sleep_gpio_port; GPIO_TypeDef *const dir_gpio_port; TIM_HandleTypeDef *const pwm_timer; @@ -66,40 +66,34 @@ typedef struct FMW_Buzzer { uint32_t timer_channel; } FMW_Buzzer; -typedef void fmw_interrupt(void *user_data); +typedef struct FMW_Hook { + void (*callback)(void *args); + void *args; +} FMW_Hook; -void fmw_motor_init(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 *motor) __attribute__((nonnull)); -void fmw_motor_enable(FMW_Motor * motor) __attribute__((nonnull)); -void fmw_motor_disable(FMW_Motor * motor) __attribute__((nonnull)); +void fmw_motor_init(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 * motor) __attribute__((nonnull)); +void fmw_motor_disable(FMW_Motor * motor) __attribute__((nonnull)); void fmw_encoder_init(FMW_Encoder encoders[], int32_t count) __attribute__((nonnull)); -FMW_Result fmw_encoder_update(FMW_Encoder *encoder) __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)); int32_t fmw_encoder_count_get(const FMW_Encoder *encoder) __attribute__((warn_unused_result, nonnull)); -int32_t fmw_pid_update(FMW_PidController *pid, float measure) __attribute__((warn_unused_result, nonnull)); - -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_update(FMW_Led *led) __attribute__((nonnull)); +int32_t fmw_pid_update(FMW_PidController *pid, float velocity) __attribute__((warn_unused_result, nonnull)); -FMW_Result fmw_buzzer_set(FMW_Buzzer buzzer[], int32_t count, bool on) __attribute__((nonnull)); +void fmw_odometry_setpoint_from_velocities(FMW_Odometry *odometry, float linear, float angular) __attribute__((nonnull)); -void fmw_result_log_uart(UART_HandleTypeDef *huart, FMW_Result result, - const char *filename, int16_t filename_length, - int32_t line) __attribute__((nonnull)); +void fmw_led_init(FMW_Led *led) __attribute__((nonnull)); +void fmw_led_update(FMW_Led *led) __attribute__((nonnull)); -FMW_Result fmw_message_receive_uart(UART_HandleTypeDef *huart, - int32_t wait_ms, FMW_Message *msg) __attribute__((warn_unused_result, nonnull)); +void fmw_buzzer_set(FMW_Buzzer buzzer[], int32_t count, bool on) __attribute__((nonnull)); -int32_t fmw_result_format(char buffer[], size_t buffer_size, - const char *filename, int16_t filename_length, - int32_t line, FMW_Result result) __attribute__((warn_unused_result, 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)); #define FMW_LED_UPDATE_PERIOD 200 #define FMW_DEBOUNCE_DELAY 200 @@ -111,7 +105,15 @@ int32_t fmw_result_format(char buffer[], size_t buffer_size, #define FMW_METERS_FROM_TICKS(Ticks, WheelCircumference, TicksPerRevolution) \ ((Ticks * WheelCircumference) / TicksPerRevolution) -#define FMW_RESULT_LOG_UART(HUART_PTR, FMW_RESULT) \ - fmw_result_log_uart((HUART_PTR), (FMW_RESULT), __FILE__, ARRLENGTH(__FILE__), __LINE__) +#define FMW_ASSERT(Cond, ...) \ + do { \ + FMW_Hook hook = { __VA_ARGS__ }; \ + if (!(Cond)) { \ + if (hook.callback) { \ + hook.callback(hook.args); \ + } \ + __builtin_trap(); \ + } \ + } while (0) #endif diff --git a/pioneer_controller/Core/Inc/firmware/fmw_messages.h b/pioneer_controller/Core/Inc/firmware/fmw_messages.h index 21a318d..fb65170 100644 --- a/pioneer_controller/Core/Inc/firmware/fmw_messages.h +++ b/pioneer_controller/Core/Inc/firmware/fmw_messages.h @@ -10,25 +10,11 @@ typedef union { float values[3]; } FMW_PidConstants; -typedef uint16_t FMW_MessageStatusCode; -enum { - FMW_MessageStatusCode_None, - - FMW_MessageStatusCode_Waiting4Config = 0, - FMW_MessageStatusCode_Running = 1, - FMW_MessageStatusCode_Error_Config = 2, - FMW_MessageStatusCode_Error_Velocity = 3, - FMW_MessageStatusCode_Fault_HBridge = 4, - - FMW_MessageStatusCode_COUNT, -}; - typedef uint8_t FMW_State; enum { FMW_State_None, FMW_State_Init, - FMW_State_Error, FMW_State_Running, FMW_State_COUNT, @@ -39,12 +25,19 @@ enum { FMW_MessageType_None, FMW_MessageType_Error, - FMW_MessageType_Run, + 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_Status, - FMW_MessageType_Velocity, + + FMW_MessageType_Run_GetStatus, + FMW_MessageType_Run_GetStatus_Response, + FMW_MessageType_Run_SetVelocity, + FMW_MessageType_Run_SetVelocity_Response, FMW_MessageType_COUNT, }; @@ -85,10 +78,8 @@ typedef struct { union { struct { - const char *filename; int32_t line; - int32_t filename_size; - FMW_Result reason; + int32_t file_size; } error; struct { @@ -110,12 +101,13 @@ typedef struct { uint32_t update_period; } config_led; + FMW_Result result; struct { - FMW_MessageStatusCode status_code; + FMW_Result result; uint16_t delta_millis; - int32_t left_ticks; - int32_t right_ticks; - } status; + int32_t ticks_left; + int32_t ticks_right; + } status_response; struct { float linear; float angular; diff --git a/pioneer_controller/Core/Src/firmware/fwm_core.c b/pioneer_controller/Core/Src/firmware/fwm_core.c index 499c928..a75c3a4 100644 --- a/pioneer_controller/Core/Src/firmware/fwm_core.c +++ b/pioneer_controller/Core/Src/firmware/fwm_core.c @@ -3,8 +3,19 @@ #include -FMW_Result fmw_message_receive_uart(UART_HandleTypeDef *huart, int32_t wait_ms, FMW_Message *msg) { - assert(wait_ms >= 0); +static struct { + FMW_Motor *motors; + int32_t motors_count; +} fmw_state; + +static void fmw_hook_assert_fail(void *_) { + if (fmw_state.motors != NULL) { + fmw_motor_brake(fmw_state.motors, fmw_state.motors_count); + } +} + +FMW_Result fmw_message_uart_receive(UART_HandleTypeDef *huart, FMW_Message *msg, int32_t wait_ms) { + FMW_ASSERT(wait_ms >= 0, .callback = fmw_hook_assert_fail); memset(msg, 0, sizeof *msg); HAL_StatusTypeDef uart_packet_status = HAL_UART_Receive(huart, (uint8_t*)msg, sizeof(*msg), wait_ms); @@ -15,87 +26,103 @@ FMW_Result fmw_message_receive_uart(UART_HandleTypeDef *huart, int32_t wait_ms, return FMW_Result_Ok; } -int32_t fmw_result_format(char buffer[], size_t buffer_size, - const char *filename, int16_t filename_length, - int32_t line, FMW_Result result) { - static const char* const variant_strings[] = { - #define X(Variant) #Variant, - FMW_RESULT_VARIANTS(X) - #undef X - }; - return snprintf(buffer, buffer_size, - "[%.*s:%ld] failed with result: %s\r\n", - filename_length, filename, line, - variant_strings[result]); -} - -void fmw_result_log_uart(UART_HandleTypeDef *huart, FMW_Result result, - const char *filename, int16_t filename_length, - int32_t line) { - char buff[512] = {0}; - int32_t length = fmw_result_format(buff, sizeof buff, filename, filename_length, line, result); - (void)HAL_UART_Transmit(huart, (uint8_t*)buff, length, HAL_MAX_DELAY); +void 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); } // ============================================================ // Motor controller void fmw_motor_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) { - assert(motors[i].sleep_gpio_port); - assert(motors[i].dir_gpio_port); - assert(motors[i].pwm_timer); - 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 || - motors[i].pwm_channel == TIM_CHANNEL_ALL); - assert(motors[i].sleep_pin == GPIO_PIN_1 || motors[i].sleep_pin == GPIO_PIN_2 || - motors[i].sleep_pin == GPIO_PIN_3 || motors[i].sleep_pin == GPIO_PIN_4 || - motors[i].sleep_pin == GPIO_PIN_5 || motors[i].sleep_pin == GPIO_PIN_6 || - motors[i].sleep_pin == GPIO_PIN_7 || motors[i].sleep_pin == GPIO_PIN_8 || - motors[i].sleep_pin == GPIO_PIN_9 || motors[i].sleep_pin == GPIO_PIN_10 || - motors[i].sleep_pin == GPIO_PIN_11 || motors[i].sleep_pin == GPIO_PIN_12 || - motors[i].sleep_pin == GPIO_PIN_13 || motors[i].sleep_pin == GPIO_PIN_14 || - motors[i].sleep_pin == GPIO_PIN_15 || motors[i].sleep_pin == GPIO_PIN_All); - assert(motors[i].dir_pin == GPIO_PIN_1 || motors[i].dir_pin == GPIO_PIN_2 || - motors[i].dir_pin == GPIO_PIN_3 || motors[i].dir_pin == GPIO_PIN_4 || - motors[i].dir_pin == GPIO_PIN_5 || motors[i].dir_pin == GPIO_PIN_6 || - motors[i].dir_pin == GPIO_PIN_7 || motors[i].dir_pin == GPIO_PIN_8 || - motors[i].dir_pin == GPIO_PIN_9 || motors[i].dir_pin == GPIO_PIN_10 || - motors[i].dir_pin == GPIO_PIN_11 || motors[i].dir_pin == GPIO_PIN_12 || - motors[i].dir_pin == GPIO_PIN_13 || motors[i].dir_pin == GPIO_PIN_14 || - motors[i].dir_pin == GPIO_PIN_15 || motors[i].dir_pin == GPIO_PIN_All); - assert(motors[i].dir_pin != motors[i].sleep_pin); + FMW_ASSERT(motors[i].sleep_gpio_port); + FMW_ASSERT(motors[i].dir_gpio_port); + FMW_ASSERT(motors[i].pwm_timer); + 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 || + motors[i].pwm_channel == TIM_CHANNEL_ALL); + FMW_ASSERT(motors[i].sleep_pin == GPIO_PIN_1 || motors[i].sleep_pin == GPIO_PIN_2 || + motors[i].sleep_pin == GPIO_PIN_3 || motors[i].sleep_pin == GPIO_PIN_4 || + motors[i].sleep_pin == GPIO_PIN_5 || motors[i].sleep_pin == GPIO_PIN_6 || + motors[i].sleep_pin == GPIO_PIN_7 || motors[i].sleep_pin == GPIO_PIN_8 || + motors[i].sleep_pin == GPIO_PIN_9 || motors[i].sleep_pin == GPIO_PIN_10 || + motors[i].sleep_pin == GPIO_PIN_11 || motors[i].sleep_pin == GPIO_PIN_12 || + motors[i].sleep_pin == GPIO_PIN_13 || motors[i].sleep_pin == GPIO_PIN_14 || + motors[i].sleep_pin == GPIO_PIN_15 || motors[i].sleep_pin == GPIO_PIN_All); + FMW_ASSERT(motors[i].dir_pin == GPIO_PIN_1 || motors[i].dir_pin == GPIO_PIN_2 || + motors[i].dir_pin == GPIO_PIN_3 || motors[i].dir_pin == GPIO_PIN_4 || + motors[i].dir_pin == GPIO_PIN_5 || motors[i].dir_pin == GPIO_PIN_6 || + motors[i].dir_pin == GPIO_PIN_7 || motors[i].dir_pin == GPIO_PIN_8 || + motors[i].dir_pin == GPIO_PIN_9 || motors[i].dir_pin == GPIO_PIN_10 || + motors[i].dir_pin == GPIO_PIN_11 || motors[i].dir_pin == GPIO_PIN_12 || + motors[i].dir_pin == GPIO_PIN_13 || motors[i].dir_pin == GPIO_PIN_14 || + motors[i].dir_pin == GPIO_PIN_15 || motors[i].dir_pin == GPIO_PIN_All); + FMW_ASSERT(motors[i].dir_pin != motors[i].sleep_pin); HAL_StatusTypeDef status = HAL_TIM_PWM_Start(motors[i].pwm_timer, motors[i].pwm_channel); - assert(status == HAL_OK); + FMW_ASSERT(status == HAL_OK); motors[i].max_dutycycle = motors[i].pwm_timer->Instance->ARR; motors[i].active = true; - fmw_motor_brake(&motors[i]); } + fmw_motor_brake(motors, count); } void fmw_motor_set_speed(FMW_Motor *motor, int32_t duty_cycle) { + FMW_ASSERT(motor->dir_gpio_port != NULL, .callback = fmw_hook_assert_fail); + FMW_ASSERT(motor->dir_pin == GPIO_PIN_1 || motor->dir_pin == GPIO_PIN_2 || + motor->dir_pin == GPIO_PIN_3 || motor->dir_pin == GPIO_PIN_4 || + motor->dir_pin == GPIO_PIN_5 || motor->dir_pin == GPIO_PIN_6 || + motor->dir_pin == GPIO_PIN_7 || motor->dir_pin == GPIO_PIN_8 || + motor->dir_pin == GPIO_PIN_9 || motor->dir_pin == GPIO_PIN_10 || + motor->dir_pin == GPIO_PIN_11 || motor->dir_pin == GPIO_PIN_12 || + motor->dir_pin == GPIO_PIN_13 || motor->dir_pin == GPIO_PIN_14 || + motor->dir_pin == GPIO_PIN_15 || motor->dir_pin == GPIO_PIN_All, + .callback = fmw_hook_assert_fail); HAL_GPIO_WritePin(motor->dir_gpio_port, motor->dir_pin, - (duty_cycle >= 0 - ? FMW_MotorDirection_Forward - : FMW_MotorDirection_Backward)); + (duty_cycle >= 0 ? FMW_MotorDirection_Forward : FMW_MotorDirection_Backward)); duty_cycle = CLAMP_TOP(ABS(duty_cycle), motor->max_dutycycle); __HAL_TIM_SET_COMPARE(motor->pwm_timer, motor->pwm_channel, duty_cycle); HAL_GPIO_WritePin(motor->sleep_gpio_port, motor->sleep_pin, GPIO_PIN_SET); } -void fmw_motor_brake(FMW_Motor *motor) { - HAL_GPIO_WritePin(motor->sleep_gpio_port, motor->sleep_pin, FMW_MotorDirection_Backward); - __HAL_TIM_SET_COMPARE(motor->pwm_timer, motor->pwm_channel, 0); +void fmw_motor_brake(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); + FMW_ASSERT(motors[i].sleep_pin == GPIO_PIN_1 || motors[i].sleep_pin == GPIO_PIN_2 || + motors[i].sleep_pin == GPIO_PIN_3 || motors[i].sleep_pin == GPIO_PIN_4 || + motors[i].sleep_pin == GPIO_PIN_5 || motors[i].sleep_pin == GPIO_PIN_6 || + motors[i].sleep_pin == GPIO_PIN_7 || motors[i].sleep_pin == GPIO_PIN_8 || + motors[i].sleep_pin == GPIO_PIN_9 || motors[i].sleep_pin == GPIO_PIN_10 || + motors[i].sleep_pin == GPIO_PIN_11 || motors[i].sleep_pin == GPIO_PIN_12 || + motors[i].sleep_pin == GPIO_PIN_13 || motors[i].sleep_pin == GPIO_PIN_14 || + motors[i].sleep_pin == GPIO_PIN_15 || motors[i].sleep_pin == GPIO_PIN_All); + 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 || + motors[i].pwm_channel == TIM_CHANNEL_ALL); + + HAL_GPIO_WritePin(motors[i].sleep_gpio_port, motors[i].sleep_pin, FMW_MotorDirection_Backward); + __HAL_TIM_SET_COMPARE(motors[i].pwm_timer, motors[i].pwm_channel, 0); + } } void fmw_motor_enable(FMW_Motor *motor) { - HAL_TIM_PWM_Start(motor->pwm_timer, motor->pwm_channel); + HAL_StatusTypeDef res = HAL_TIM_PWM_Start(motor->pwm_timer, motor->pwm_channel); + FMW_ASSERT(res == HAL_OK); motor->active = true; } void fmw_motor_disable(FMW_Motor *motor) { - HAL_TIM_PWM_Stop(motor->pwm_timer, motor->pwm_channel); + HAL_StatusTypeDef res = HAL_TIM_PWM_Stop(motor->pwm_timer, motor->pwm_channel); + FMW_ASSERT(res == HAL_OK, .callback = fmw_hook_assert_fail); motor->active = false; } @@ -103,73 +130,66 @@ void fmw_motor_disable(FMW_Motor *motor) { // Encoder void fmw_encoder_init(FMW_Encoder encoders[], int32_t count) { for (int32_t i = 0; i < count; ++i) { - assert(encoders[i].timer != NULL); - assert(encoders[i].ticks_per_revolution > 0); - assert(encoders[i].wheel_circumference > 0.f); + FMW_ASSERT(encoders[i].timer != NULL); + FMW_ASSERT(encoders[i].ticks_per_revolution > 0); + FMW_ASSERT(encoders[i].wheel_circumference > 0.f); encoders[i].previous_millis = 0; encoders[i].current_millis = 0; encoders[i].ticks = 0; HAL_StatusTypeDef status = HAL_TIM_Encoder_Start(encoders[i].timer, TIM_CHANNEL_ALL); - assert(status == HAL_OK); + FMW_ASSERT(status == HAL_OK); fmw_encoder_count_reset(&encoders[i]); encoders[i].current_millis = HAL_GetTick(); } } -FMW_Result fmw_encoder_update(FMW_Encoder *encoder) { +void fmw_encoder_update(FMW_Encoder *encoder) { encoder->previous_millis = encoder->current_millis; encoder->current_millis = HAL_GetTick(); encoder->ticks = fmw_encoder_count_get(encoder); fmw_encoder_count_reset(encoder); - if (!(__builtin_expect(encoder->current_millis >= encoder->previous_millis, true))) { - return FMW_Result_Error_Encoder_GetTick; - } else { - return FMW_Result_Ok; - } + FMW_ASSERT(encoder->current_millis >= encoder->previous_millis, .callback = fmw_hook_assert_fail); } float fmw_encoder_get_linear_velocity(const FMW_Encoder *encoder) { float deltatime = encoder->current_millis - encoder->previous_millis; - if (!(__builtin_expect(deltatime > 0.f, true))) { - return -1.f; - } else { - float meters = FMW_METERS_FROM_TICKS(encoder->ticks, - encoder->wheel_circumference, - encoder->ticks_per_revolution); - assert(meters >= 0.f); - float linear_velocity = meters / (deltatime / 1000.f); - assert(linear_velocity >= 0.f); - return linear_velocity; - } + FMW_ASSERT(deltatime > 0.f, .callback = fmw_hook_assert_fail); + float meters = FMW_METERS_FROM_TICKS(encoder->ticks, + encoder->wheel_circumference, + encoder->ticks_per_revolution); + FMW_ASSERT(meters >= 0.f, .callback = fmw_hook_assert_fail); + float linear_velocity = meters / (deltatime / 1000.f); + FMW_ASSERT(linear_velocity >= 0.f, .callback = fmw_hook_assert_fail); + return linear_velocity; } void fmw_encoder_count_reset(FMW_Encoder *encoder) { - assert(encoder->timer); + FMW_ASSERT(encoder->timer, .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) { - assert(encoder->timer); + FMW_ASSERT(encoder->timer, .callback = fmw_hook_assert_fail); return (int32_t)__HAL_TIM_GET_COUNTER(encoder->timer) - (encoder->timer->Init.Period / 2); } // ============================================================ // Odometry void fmw_odometry_setpoint_from_velocities(FMW_Odometry *odometry, float linear, float angular) { + FMW_ASSERT(odometry->baseline > 0.f, .callback = fmw_hook_assert_fail); odometry->setpoint_left = linear - (odometry->baseline * angular) / 2; odometry->setpoint_right = linear + (odometry->baseline * angular) / 2; } // ============================================================ // PID -int32_t fmw_pid_update(FMW_PidController *pid, float measure) { - pid->error = pid->setpoint - measure; - +int32_t fmw_pid_update(FMW_PidController *pid, float velocity) { + pid->error = pid->setpoint - velocity; float output = pid->error * pid->ks.proportional; - //integral term without windup + // integral term without windup pid->error_sum += pid->error; output += pid->error_sum * pid->ks.integral; @@ -185,25 +205,38 @@ int32_t fmw_pid_update(FMW_PidController *pid, float measure) { // ============================================================ // LEDs void fmw_led_init(FMW_Led *led) { - assert(led->timer); - assert(led->adc); - 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 || - led->timer_channel == TIM_CHANNEL_ALL); - assert(led->voltage_red > 0.f); - assert(led->voltage_orange > 0.f); - assert(led->voltage_hysteresis >= 0.f); - assert(led->state < FMW_LedState_COUNT); + FMW_ASSERT(led->timer); + FMW_ASSERT(led->adc); + 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 || + led->timer_channel == TIM_CHANNEL_ALL); + FMW_ASSERT(led->voltage_red > 0.f); + FMW_ASSERT(led->voltage_orange > 0.f); + FMW_ASSERT(led->voltage_hysteresis >= 0.f); + FMW_ASSERT(led->state < FMW_LedState_COUNT); HAL_StatusTypeDef status = HAL_TIM_PWM_Start(led->timer, led->timer_channel); - assert(status == HAL_OK); + FMW_ASSERT(status == HAL_OK); __HAL_TIM_SET_COMPARE(led->timer, led->timer_channel, 0); } void fmw_led_update(FMW_Led *led) { - HAL_ADC_Start(led->adc); - HAL_ADC_PollForConversion(led->adc, HAL_MAX_DELAY); + FMW_ASSERT(led->timer && led->adc, .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 || + led->timer_channel == TIM_CHANNEL_ALL, .callback = fmw_hook_assert_fail); + FMW_ASSERT(led->voltage_red > 0.f, .callback = fmw_hook_assert_fail); + FMW_ASSERT(led->voltage_orange > 0.f, .callback = fmw_hook_assert_fail); + FMW_ASSERT(led->voltage_hysteresis >= 0.f, .callback = fmw_hook_assert_fail); + FMW_ASSERT(led->state < FMW_LedState_COUNT, .callback = fmw_hook_assert_fail); + + HAL_StatusTypeDef adc_start_res = HAL_ADC_Start(led->adc); + FMW_ASSERT(adc_start_res == HAL_OK, .callback = fmw_hook_assert_fail); + HAL_StatusTypeDef adc_poll_res = HAL_ADC_PollForConversion(led->adc, HAL_MAX_DELAY); + FMW_ASSERT(adc_poll_res == HAL_OK, .callback = fmw_hook_assert_fail); + uint32_t adc_val = HAL_ADC_GetValue(led->adc); float v_adc = ((float)adc_val / FMW_ADC_RESOLUTION) * FMW_V_REF; float vin = v_adc * FMW_VIN_SCALE_FACTOR; @@ -254,8 +287,8 @@ void fmw_led_update(FMW_Led *led) { // ============================================================ // Buzzers // NOTE(lb): replace bool with uint8_t bitmask? -FMW_Result fmw_buzzer_set(FMW_Buzzer buzzer[], int32_t count, bool on) { - if (count < 1) { return FMW_Result_Error_InvalidArguments; } +void fmw_buzzer_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; if (on) { @@ -265,7 +298,6 @@ FMW_Result fmw_buzzer_set(FMW_Buzzer buzzer[], int32_t count, bool on) { } else { res = HAL_TIM_PWM_Stop(buzzer[i].timer, buzzer[i].timer_channel); } - if (res != HAL_OK) { return FMW_Result_Error_Buzzer_Timer; } + FMW_ASSERT(res == HAL_OK, .callback = fmw_hook_assert_fail); } - return FMW_Result_Ok; } diff --git a/pioneer_controller/Core/Src/main.c b/pioneer_controller/Core/Src/main.c index 914a98b..7c47c7a 100644 --- a/pioneer_controller/Core/Src/main.c +++ b/pioneer_controller/Core/Src/main.c @@ -32,7 +32,7 @@ /* Private typedef -----------------------------------------------------------*/ /* USER CODE BEGIN PTD */ -FMW_Result message_handler(FMW_Message *msg) +FMW_Result message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) __attribute__((warn_unused_result, nonnull)); /* USER CODE END PTD */ @@ -781,13 +781,11 @@ static void MX_GPIO_Init(void) /* USER CODE BEGIN 4 */ void start(void) { for (FMW_Message msg = {0}; fmw_state == FMW_State_Init; ) { - FMW_Result res = fmw_message_receive_uart(UART_MESSANGER_HANDLE, 180 * 1000, &msg); - if (res != FMW_Result_Ok) { - FMW_RESULT_LOG_UART(UART_MESSANGER_HANDLE, res); - } else { - res = message_handler(&msg); - if (res != FMW_Result_Ok) { FMW_RESULT_LOG_UART(UART_MESSANGER_HANDLE, res); } - } + FMW_Result res = fmw_message_uart_receive(UART_MESSANGER_HANDLE, &msg, 180 * 1000); + FMW_Message response = {0}; + response.header.type = FMW_MessageType_Response; + response.result = (res == FMW_Result_Ok) ? message_handler(&msg, &hcrc) : res; + fmw_message_uart_send(UART_MESSANGER_HANDLE, &hcrc, &response, HAL_MAX_DELAY); } fmw_encoder_init(encoders.values, ENCODER_COUNT); @@ -797,11 +795,11 @@ void start(void) { // Right and left motors have the same parameters pid_max = (int32_t)htim4.Instance->ARR; pid_min = -pid_max; - assert(pid_max > pid_min); + FMW_ASSERT(pid_max > pid_min); // Enables TIM6 interrupt (used for PID control) HAL_StatusTypeDef timer_status = HAL_TIM_Base_Start_IT(&htim6); - assert(timer_status == HAL_OK); + FMW_ASSERT(timer_status == HAL_OK); // Enables UART RX interrupt HAL_UART_Receive_DMA(UART_MESSANGER_HANDLE, (uint8_t*)&run_msg, sizeof run_msg); @@ -815,21 +813,21 @@ void start(void) { } } -FMW_Result message_handler(FMW_Message *msg) { +FMW_Result message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) { // NOTE(lb): the `msg->header.crc != -1` checks are just because i haven't // implemented CRC into the program that sends these messages. // i also don't know if the code to calculate CRC is correct (probably isn't). if (msg->header.crc != -1) { uint32_t crc_received = msg->header.crc; msg->header.crc = 0; - uint32_t crc_computed = HAL_CRC_Calculate(&hcrc, (uint32_t*)msg, sizeof *msg); + uint32_t crc_computed = HAL_CRC_Calculate(hcrc, (uint32_t*)msg, sizeof *msg); if (!(crc_computed == crc_received)) { return FMW_Result_Error_UART_Crc; } } switch (fmw_state) { case FMW_State_Init: { switch (msg->header.type) { - case FMW_MessageType_Run: { + case FMW_MessageType_StateChange_Run: { fmw_state = FMW_State_Running; } break; case FMW_MessageType_Config_Robot: { @@ -866,8 +864,8 @@ FMW_Result message_handler(FMW_Message *msg) { pled.voltage_hysteresis = msg->config_led.voltage_hysteresis; led_update_period = msg->config_led.update_period; } break; - case FMW_MessageType_Status: // NOTE(lb): allow status messages in init mode? - case FMW_MessageType_Velocity: { + case FMW_MessageType_Run_GetStatus: // NOTE(lb): allow status messages in init mode? + case FMW_MessageType_Run_SetVelocity: { return FMW_Result_Error_Command_NotAvailable; } break; default: { @@ -877,7 +875,7 @@ FMW_Result message_handler(FMW_Message *msg) { } break; case FMW_State_Running: { switch (msg->header.type) { - case FMW_MessageType_Status: { // TODO(lb): this should be `GetStatus` or something like that. + 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); ticks_left = ticks_right = 0; @@ -887,28 +885,22 @@ FMW_Result message_handler(FMW_Message *msg) { float time_millis_delta = time_millis_current - time_millis_previous; time_millis_previous = time_millis_current; - // NOTE(lb): Does a status response need to be its own message or - // is just logging fine? Is the workstation program interactive - // (is there a user choosing which messages to send) - // or is automated? And if it is automated wouldn't - // it want to know if the board actually received its message? - // If the build process wasn't so overcomplicated i could just add - // a compilation flag and switch between both at comptime. - - char buffer[128] = {0}; - int32_t buffer_size = snprintf(buffer, ARRLENGTH(buffer), "time_millis_delta : %f\n" - "ticks_left : %ld\n" - "ticks_right : %ld\n", - time_millis_delta, current_ticks_left, current_ticks_right); - (void)HAL_UART_Transmit(UART_MESSANGER_HANDLE, (uint8_t*)buffer, buffer_size, HAL_MAX_DELAY); + 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.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); } break; - case FMW_MessageType_Velocity: { + 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; } break; - case FMW_MessageType_Run: + case FMW_MessageType_StateChange_Run: case FMW_MessageType_Config_Robot: case FMW_MessageType_Config_PID: case FMW_MessageType_Config_LED: { @@ -933,12 +925,12 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { { // PID control fmw_encoder_update(&encoders.left); float velocity_left = fmw_encoder_get_linear_velocity(&encoders.left); - int dutycycle_left = fmw_pid_update(&pid_left, velocity_left); + int32_t dutycycle_left = fmw_pid_update(&pid_left, velocity_left); fmw_encoder_update(&encoders.right); float velocity_right = fmw_encoder_get_linear_velocity(&encoders.right); - int dutycycle_right = fmw_pid_update(&pid_right, velocity_right); - int dutycycle_cross = fmw_pid_update(&pid_cross, velocity_left - velocity_right); + int32_t dutycycle_right = fmw_pid_update(&pid_right, velocity_right); + int32_t dutycycle_cross = fmw_pid_update(&pid_cross, velocity_left - velocity_right); dutycycle_left += dutycycle_cross; dutycycle_right -= dutycycle_cross; @@ -950,11 +942,10 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) { if (huart == UART_MESSANGER_HANDLE) { - // NOTE(lb): i don't think another interrupt of this kind can occur - // while i'm still handling the previous one so casting away - // volatile should be fine. - FMW_Result res = message_handler((FMW_Message*)&run_msg); - if (res != FMW_Result_Ok) { FMW_RESULT_LOG_UART(huart, res); } + FMW_Message response = {0}; + response.header.type = FMW_MessageType_Response; + response.result = message_handler((FMW_Message*)&run_msg, &hcrc); + fmw_message_uart_send(UART_MESSANGER_HANDLE, &hcrc, &response, HAL_MAX_DELAY); // NOTE(lb): listen for the next message. HAL_UART_Receive_DMA(huart, (uint8_t*)&run_msg, sizeof run_msg); @@ -1005,9 +996,9 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { /* HAL_UART_Transmit(&huart3, (uint8_t*)msg, strlen(msg), HAL_MAX_DELAY); */ // NOTE(lb): lol strlen } else { - // NOTE(lb): is it safe to assert here since the motors aren't running? - assert(!motors.left.active); - assert(!motors.right.active); + // NOTE(lb): is it safe to FMW_ASSERT here since the motors aren't running? + FMW_ASSERT(!motors.left.active); + FMW_ASSERT(!motors.right.active); fmw_motor_enable(&motors.left); fmw_motor_enable(&motors.right); @@ -1021,8 +1012,7 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { } break; case fault1_Pin: case fault2_Pin: { - fmw_motor_brake(&motors.left); - fmw_motor_brake(&motors.right); + fmw_motor_brake(motors.values, ARRLENGTH(motors.values)); // stop TIM6 interrupt (used for PID control) HAL_TIM_Base_Stop_IT(&htim6); /* otto_status = MessageStatusCode_Fault_HBridge; */ @@ -1039,7 +1029,6 @@ void Error_Handler(void) { /* USER CODE BEGIN Error_Handler_Debug */ /* User can add his own implementation to report the HAL error return state */ - /* USER CODE END Error_Handler_Debug */ } #ifdef USE_FULL_ASSERT diff --git a/pioneer_workstation/src/main.c b/pioneer_workstation/src/main.c index c4a9692..5bdf32f 100644 --- a/pioneer_workstation/src/main.c +++ b/pioneer_workstation/src/main.c @@ -12,21 +12,30 @@ // TODO(lb): implement CRC static int serial_open(char *portname) { - int fd = open(portname, O_RDWR | O_NOCTTY | O_SYNC); + int fd = open(portname, O_RDWR | O_NOCTTY); assert(fd >= 0); - tcflush(fd, TCIOFLUSH); struct termios tty; - assert(!tcgetattr(fd, &tty)); - cfsetospeed(&tty, B115200); // output baud rate - cfsetispeed(&tty, B115200); // input baud rate - tty.c_cflag = (tty.c_cflag & (tcflag_t)~CSIZE) | CS8; // 8-bit chars - tty.c_iflag &= (tcflag_t)~IGNBRK; // disable break processing - tty.c_cc[VMIN] = 0; // read doesn't block - tty.c_cc[VTIME] = 20; // 2.0 seconds read timeout - tty.c_cflag &= (tcflag_t)~(PARENB | PARODD); // no parity - tty.c_cflag &= (tcflag_t)~CSTOPB; // 1 stop bit - assert(!tcsetattr(fd, TCSANOW, &tty)); + tcgetattr(fd, &tty); + + cfsetispeed(&tty, B115200); + cfsetospeed(&tty, B115200); + + tty.c_cflag &= (tcflag_t)~PARENB; + tty.c_cflag &= (tcflag_t)~CSTOPB; + tty.c_cflag &= (tcflag_t)~CRTSCTS; + tty.c_cflag |= CS8 | CLOCAL | CREAD; + + tty.c_lflag = 0; + tty.c_iflag = 0; + tty.c_oflag = 0; + + tty.c_cc[VMIN] = 1; + tty.c_cc[VTIME] = 0; + + tcflush(fd, TCIFLUSH); + tcsetattr(fd, TCSANOW, &tty); + return fd; } @@ -34,7 +43,8 @@ int main(void) { int fd = serial_open("/dev/ttyACM0"); assert(fd); -#if 1 + FMW_Message response = {0}; + FMW_Message msg_config_robot = { .header = { .type = FMW_MessageType_Config_Robot, @@ -42,13 +52,19 @@ int main(void) { }, .config_robot = { .baseline = 2.3f, - .left_wheel_circumference = 4.f, - .left_ticks_per_revolution = 250, - .right_wheel_circumference = 3.2f, - .right_ticks_per_revolution = 350, + .wheel_circumference_left = 4.f, + .wheel_circumference_right = 3.2f, + .ticks_per_revolution_left = 250, + .ticks_per_revolution_right = 350, }, }; write(fd, &msg_config_robot, 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); + assert(response.result == FMW_Result_Ok); FMW_Message msg_config_pid = { .header = { @@ -74,61 +90,26 @@ int main(void) { }, }; write(fd, &msg_config_pid, 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); + assert(response.result == FMW_Result_Ok); FMW_Message msg_run = { .header = { - .type = FMW_MessageType_Run, + .type = FMW_MessageType_StateChange_Run, .crc = (uint32_t)-1, }, }; write(fd, &msg_run, sizeof(FMW_Message)); -#else - FMW_Message msgs[3] = { - { - .header = { - .type = FMW_MessageType_Config_Robot, - .crc = (uint32_t)-1, - }, - .config_robot = { - .baseline = 2.3f, - .left_wheel_circumference = 4.f, - .left_ticks_per_revolution = 250, - .right_wheel_circumference = 3.2f, - .right_ticks_per_revolution = 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_Run, - .crc = (uint32_t)-1, - }, - }, - }; - write(fd, &msgs, 3 * sizeof(FMW_Message)); -#endif + 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); + assert(response.result == FMW_Result_Ok); close(fd); }