#include "main.h"
#include "firmware/fmw_inc.h"
+#include <string.h>
+
+FMW_Result fmw_message_receive_uart(UART_HandleTypeDef *huart, int32_t wait_ms, FMW_Message *msg) {
+ assert(wait_ms >= 0);
+ memset(msg, 0, sizeof *msg);
+
+ HAL_StatusTypeDef uart_packet_status = HAL_UART_Receive(huart, (uint8_t*)msg, sizeof(*msg), wait_ms);
+ if (!(uart_packet_status == HAL_OK)) { return FMW_Result_Error_UART_ReceiveTimeoutElapsed; }
+ if (!(msg->header.type > FMW_MessageType_None && msg->header.type < FMW_MessageType_COUNT)) {
+ return FMW_Result_Error_Command_NotRecognized;
+ }
+ 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);
+ HAL_UART_Transmit(huart, (uint8_t*)buff, length, HAL_MAX_DELAY);
+}
+
// ============================================================
// Motor controller
-void fmw_motor_init(FMW_Motor motors[FMW_MOTOR_COUNT]) {
- for (int32_t i = 0; i < FMW_MOTOR_COUNT; ++ i) {
- HAL_TIM_PWM_Start(motors[i].pwm_timer, motors[i].pwm_channel);
+void fmw_motor_init(FMW_Motor motors[], int32_t 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);
+
+ HAL_StatusTypeDef status = HAL_TIM_PWM_Start(motors[i].pwm_timer, motors[i].pwm_channel);
+ assert(status == HAL_OK);
motors[i].max_dutycycle = motors[i].pwm_timer->Instance->ARR;
motors[i].active = true;
fmw_motor_brake(&motors[i]);
// ============================================================
// Encoder
-void fmw_encoder_init(FMW_Encoder encoders[FMW_ENCODER_COUNT]) {
- for (int32_t i = 0; i < FMW_ENCODER_COUNT; ++i) {
- HAL_TIM_Encoder_Start(encoders[i].timer, TIM_CHANNEL_ALL);
- fmw_encoder_count_reset(&encoders[i]);
+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);
+
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_encoder_count_reset(&encoders[i]);
encoders[i].current_millis = HAL_GetTick();
}
}
-void fmw_encoder_update(FMW_Encoder *encoder) {
+FMW_Result 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;
+ }
}
float fmw_encoder_get_linear_velocity(const FMW_Encoder *encoder) {
- float meters = FMW_METERS_FROM_TICKS(encoder->ticks,
- encoder->wheel_circumference,
- encoder->ticks_per_revolution);
float deltatime = encoder->current_millis - encoder->previous_millis;
- float linear_velocity = deltatime > 0.f ? (meters / (deltatime / 1000.f)) : 0.f;
- return linear_velocity;
+ 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;
+ }
}
void fmw_encoder_count_reset(FMW_Encoder *encoder) {
+ assert(encoder->timer);
__HAL_TIM_SET_COUNTER(encoder->timer, (encoder->timer->Init.Period / 2));
}
int32_t fmw_encoder_count_get(const FMW_Encoder *encoder) {
+ assert(encoder->timer);
return (int32_t)__HAL_TIM_GET_COUNTER(encoder->timer) - (encoder->timer->Init.Period / 2);
}
// ============================================================
// Odometry
-void odometry_setpoint_from_cmdvel(FMW_Odometry *odom, float linear_vel, float angular_vel) {
- odom->setpoint_left = linear_vel - (odom->baseline * angular_vel) / 2;
- odom->setpoint_right = linear_vel + (odom->baseline * angular_vel) / 2;
+void fmw_odometry_setpoint_from_velocities(FMW_Odometry *odometry, float linear, float angular) {
+ odometry->setpoint_left = linear - (odometry->baseline * angular) / 2;
+ odometry->setpoint_right = linear + (odometry->baseline * angular) / 2;
}
// ============================================================
// ============================================================
// LEDs
void fmw_led_init(FMW_Led *led) {
- HAL_TIM_PWM_Start(led->timer, led->timer_channel);
+ 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);
+
+ HAL_StatusTypeDef status = HAL_TIM_PWM_Start(led->timer, led->timer_channel);
+ assert(status == HAL_OK);
__HAL_TIM_SET_COMPARE(led->timer, led->timer_channel, 0);
}
\r
/* Private typedef -----------------------------------------------------------*/\r
/* USER CODE BEGIN PTD */\r
-\r
+FMW_Result message_handler(FMW_Message *msg)\r
+ __attribute__((warn_unused_result, nonnull));\r
/* USER CODE END PTD */\r
\r
/* Private define ------------------------------------------------------------*/\r
\r
/* Private macro -------------------------------------------------------------*/\r
/* USER CODE BEGIN PM */\r
-\r
+#define MOTOR_COUNT 2\r
+#define ENCODER_COUNT 2\r
/* USER CODE END PM */\r
\r
/* Private variables ---------------------------------------------------------*/\r
\r
/* USER CODE BEGIN PV */\r
\r
+#define UART_MESSANGER_HANDLE (&huart3)\r
+\r
// TODO(lb): fill with sensible default\r
static union {\r
- FMW_Encoder values[2];\r
+ FMW_Encoder values[ENCODER_COUNT];\r
struct {\r
FMW_Encoder right;\r
FMW_Encoder left;\r
};\r
\r
static union {\r
- FMW_Motor values[2];\r
+ FMW_Motor values[MOTOR_COUNT];\r
struct {\r
FMW_Motor right;\r
FMW_Motor left;\r
static volatile uint8_t tx_done_flag = 1;\r
/* volatile MessageStatusCode otto_status = MessageStatusCode_Waiting4Config; */\r
\r
+static volatile FMW_Message run_msg = {0};\r
+\r
static volatile uint32_t time_aux_press = 0;\r
static volatile uint32_t time_aux2_press = 0;\r
static volatile uint32_t time_last_motors = 0;\r
/* USER CODE BEGIN 0 */\r
/* USER CODE END 0 */\r
\r
-/**\r
- * @brief The application entry point.\r
- * @retval int\r
- */\r
int main(void)\r
{\r
\r
\r
/* USER CODE BEGIN 4 */\r
void start(void) {\r
- FMW_Message msg = {0};\r
- do {\r
- FMW_Result res = fmw_message_receive_uart(&huart3, 180 * 1000, &msg);\r
+ for (FMW_Message msg = {0}; fmw_state == FMW_State_Init; ) {\r
+ FMW_Result res = fmw_message_receive_uart(UART_MESSANGER_HANDLE, 180 * 1000, &msg);\r
if (res != FMW_Result_Ok) {\r
- FMW_RESULT_LOG_UART(&huart3, res);\r
- continue;\r
+ FMW_RESULT_LOG_UART(UART_MESSANGER_HANDLE, res);\r
+ } else {\r
+ res = message_handler(&msg);\r
+ if (res != FMW_Result_Ok) { FMW_RESULT_LOG_UART(UART_MESSANGER_HANDLE, res); }\r
}\r
+ }\r
\r
- res = fmw_message_handle(FMW_State_Init, &msg);\r
- if (res != FMW_Result_Ok) {\r
- FMW_RESULT_LOG_UART(&huart3, res);\r
- continue;\r
- }\r
- } while(fmw_state == FMW_State_Init);\r
-\r
- fmw_encoder_init(encoders.values);\r
- fmw_motor_init(motors.values);\r
+ fmw_encoder_init(encoders.values, ENCODER_COUNT);\r
+ fmw_motor_init(motors.values, MOTOR_COUNT);\r
fmw_led_init(&pled);\r
\r
- //right and left motors have the same parameters\r
+ // Right and left motors have the same parameters\r
pid_max = (int32_t)htim4.Instance->ARR;\r
pid_min = -pid_max;\r
+ assert(pid_max > pid_min);\r
\r
- //Enables TIM6 interrupt (used for PID control)\r
- HAL_TIM_Base_Start_IT(&htim6);\r
+ // Enables TIM6 interrupt (used for PID control)\r
+ HAL_StatusTypeDef timer_status = HAL_TIM_Base_Start_IT(&htim6);\r
+ assert(timer_status == HAL_OK);\r
\r
-#if 0\r
- //Enables UART RX interrupt\r
- HAL_UART_Receive_DMA(&huart6, (uint8_t*) &vel_msg, 12);\r
-#endif\r
+ // Enables UART RX interrupt\r
+ HAL_UART_Receive_DMA(UART_MESSANGER_HANDLE, (uint8_t*)&run_msg, sizeof run_msg);\r
\r
for (uint32_t time_last_led_update = 0;;) {\r
uint32_t time_now = HAL_GetTick();\r
}\r
}\r
\r
-FMW_Result fmw_message_receive_uart(UART_HandleTypeDef *huart, int32_t wait_ms, FMW_Message *msg) {\r
- if (!(wait_ms >= 0)) { return FMW_Result_Error_UART_NegativeTimeout; }\r
-\r
- memset(msg, 0, sizeof *msg);\r
-\r
- HAL_StatusTypeDef uart_packet_status = HAL_UART_Receive(huart, (uint8_t*)msg, sizeof(msg), wait_ms);\r
- if (!(uart_packet_status == HAL_OK)) { return FMW_Result_Error_UART_ReceiveTimeoutElapsed; }\r
- if (!(msg->header.type > FMW_MessageType_None && msg->header.type < FMW_MessageType_COUNT)) {\r
- return FMW_Result_Error_Command_NotRecognized;\r
- }\r
- return FMW_Result_Ok;\r
-}\r
-\r
-FMW_Result fmw_message_handle(FMW_State state, const FMW_Message *msg) {\r
- if (!(state > FMW_State_None && state < FMW_State_COUNT)) { return FMW_Result_Error_MessageHandler_InvalidState; }\r
-\r
+FMW_Result message_handler(FMW_Message *msg) {\r
// NOTE(lb): the `msg->header.crc != -1` checks are just because i haven't\r
// implemented CRC into the program that sends these messages.\r
- switch (state) {\r
+ // i also don't know if the code to calculate CRC is correct.\r
+ if (msg->header.crc != -1) {\r
+ uint32_t crc_received = msg->header.crc;\r
+ msg->header.crc = 0;\r
+ uint32_t crc_computed = HAL_CRC_Calculate(&hcrc, (uint32_t*)msg, sizeof *msg);\r
+ if (!(crc_computed == crc_received)) { return FMW_Result_Error_UART_Crc; }\r
+ }\r
+\r
+ switch (fmw_state) {\r
case FMW_State_Init: {\r
switch (msg->header.type) {\r
case FMW_MessageType_Run: {\r
- if (msg->header.crc != -1) {\r
- uint32_t crc_computed = HAL_CRC_Calculate(&hcrc, (uint32_t*)msg, sizeof msg->header.type);\r
- if (!(crc_computed == msg->header.crc)) { return FMW_Result_Error_UART_Crc; }\r
- }\r
fmw_state = FMW_State_Running;\r
} break;\r
case FMW_MessageType_Config_Robot: {\r
- if (msg->header.crc != -1) {\r
- uint32_t crc_computed = HAL_CRC_Calculate(&hcrc, (uint32_t*)msg, sizeof msg->header.type + sizeof msg->config_robot);\r
- if (!(crc_computed == msg->header.crc)) { return FMW_Result_Error_UART_Crc; }\r
- }\r
-\r
if (!(msg->config_robot.baseline > 0.f)) {\r
return FMW_Result_Error_MessageHandler_Init_NonPositiveBaseline;\r
}\r
- if (!(msg->config_robot.wheel_circumference_left > 0.f ||\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
}\r
- if (!(msg->config_robot.ticks_per_revolution_left >= FMW_MIN_ACCEPTABLE_TICKS_PER_REVOLUTION &&\r
- msg->config_robot.ticks_per_revolution_left < FMW_MAX_ACCEPTABLE_TICKS_PER_REVOLUTION)) {\r
- return FMW_Result_Error_MessageHandler_Init_InvalidTicksPerRevolution;\r
- }\r
- if (!(msg->config_robot.ticks_per_revolution_right >= FMW_MIN_ACCEPTABLE_TICKS_PER_REVOLUTION &&\r
- msg->config_robot.ticks_per_revolution_right < FMW_MAX_ACCEPTABLE_TICKS_PER_REVOLUTION)) {\r
- return FMW_Result_Error_MessageHandler_Init_InvalidTicksPerRevolution;\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
}\r
\r
- odometry.baseline = msg->config_robot.baseline;\r
- encoders.left.wheel_circumference = msg->config_robot.wheel_circumference_left;\r
- encoders.left.ticks_per_revolution = msg->config_robot.ticks_per_revolution_left;\r
- encoders.right.wheel_circumference = msg->config_robot.wheel_circumference_right;\r
- encoders.right.ticks_per_revolution = msg->config_robot.ticks_per_revolution_right;\r
+ odometry.baseline = msg->config_robot.baseline;\r
+ encoders.left.wheel_circumference = msg->config_robot.wheel_circumference_left;\r
+ encoders.left.ticks_per_revolution = msg->config_robot.ticks_per_revolution_left;\r
+ encoders.right.wheel_circumference = msg->config_robot.wheel_circumference_right;\r
+ encoders.right.ticks_per_revolution = msg->config_robot.ticks_per_revolution_right;\r
} break;\r
case FMW_MessageType_Config_PID: {\r
- if (msg->header.crc != -1) {\r
- uint32_t crc_computed = HAL_CRC_Calculate(&hcrc, (uint32_t*)msg, sizeof msg->header.type + sizeof msg->config_pid);\r
- if (!(crc_computed == msg->header.crc)) { return FMW_Result_Error_UART_Crc; }\r
- }\r
-\r
- if (!(msg->config_pid.left.proportional >= FMW_MIN_ACCEPTABLE_PID_PROPORTIONAL &&\r
- msg->config_pid.left.proportional < FMW_MAX_ACCEPTABLE_PID_PROPORTIONAL)) {\r
- return FMW_Result_Error_MessageHandler_Init_InvalidPIDProportionalConstant;\r
- }\r
- if (!(msg->config_pid.cross.proportional >= FMW_MIN_ACCEPTABLE_PID_PROPORTIONAL &&\r
- msg->config_pid.cross.proportional < FMW_MAX_ACCEPTABLE_PID_PROPORTIONAL)) {\r
- return FMW_Result_Error_MessageHandler_Init_InvalidPIDProportionalConstant;\r
- }\r
- if (!(msg->config_pid.right.proportional >= FMW_MIN_ACCEPTABLE_PID_PROPORTIONAL &&\r
- msg->config_pid.right.proportional < FMW_MAX_ACCEPTABLE_PID_PROPORTIONAL)) {\r
- return FMW_Result_Error_MessageHandler_Init_InvalidPIDProportionalConstant;\r
- }\r
-\r
- if (!(msg->config_pid.left.integral >= FMW_MIN_ACCEPTABLE_PID_INTEGRAL &&\r
- msg->config_pid.left.integral < FMW_MAX_ACCEPTABLE_PID_INTEGRAL)) {\r
- return FMW_Result_Error_MessageHandler_Init_InvalidPIDIntegralConstant;\r
- }\r
- if (!(msg->config_pid.cross.integral >= FMW_MIN_ACCEPTABLE_PID_INTEGRAL &&\r
- msg->config_pid.cross.integral < FMW_MAX_ACCEPTABLE_PID_INTEGRAL)) {\r
- return FMW_Result_Error_MessageHandler_Init_InvalidPIDIntegralConstant;\r
- }\r
- if (!(msg->config_pid.right.integral >= FMW_MIN_ACCEPTABLE_PID_INTEGRAL &&\r
- msg->config_pid.right.integral < FMW_MAX_ACCEPTABLE_PID_INTEGRAL)) {\r
- return FMW_Result_Error_MessageHandler_Init_InvalidPIDIntegralConstant;\r
- }\r
-\r
- if (!(msg->config_pid.left.derivative >= FMW_MIN_ACCEPTABLE_PID_DERIVATIVE &&\r
- msg->config_pid.left.derivative < FMW_MAX_ACCEPTABLE_PID_DERIVATIVE)) {\r
- return FMW_Result_Error_MessageHandler_Init_InvalidPIDDerivativeConstant;\r
- }\r
- if (!(msg->config_pid.cross.derivative >= FMW_MIN_ACCEPTABLE_PID_DERIVATIVE &&\r
- msg->config_pid.cross.derivative < FMW_MAX_ACCEPTABLE_PID_DERIVATIVE)) {\r
- return FMW_Result_Error_MessageHandler_Init_InvalidPIDDerivativeConstant;\r
- }\r
- if (!(msg->config_pid.right.derivative >= FMW_MIN_ACCEPTABLE_PID_DERIVATIVE &&\r
- msg->config_pid.right.derivative < FMW_MAX_ACCEPTABLE_PID_DERIVATIVE)) {\r
- return FMW_Result_Error_MessageHandler_Init_InvalidPIDDerivativeConstant;\r
- }\r
-\r
memcpy(&pid_left.ks, &msg->config_pid.left, sizeof pid_left.ks);\r
memcpy(&pid_right.ks, &msg->config_pid.right, sizeof pid_right.ks);\r
memcpy(&pid_cross.ks, &msg->config_pid.cross, sizeof pid_cross.ks);\r
} break;\r
case FMW_MessageType_Config_LED: {\r
- if (msg->header.crc != -1) {\r
- uint32_t crc_computed = HAL_CRC_Calculate(&hcrc, (uint32_t*)msg, sizeof msg->header.type + sizeof msg->config_led);\r
- if (!(crc_computed == msg->header.crc)) { return FMW_Result_Error_UART_Crc; }\r
- }\r
-\r
- if (!(msg->config_led.voltage_red >= FMW_MIN_ACCEPTABLE_LED_VOLTAGE &&\r
- msg->config_led.voltage_red < FMW_MAX_ACCEPTABLE_LED_VOLTAGE)) {\r
- return FMW_Result_Error_MessageHandler_Init_InvalidLEDVoltage;\r
- }\r
- if (!(msg->config_led.voltage_orange >= FMW_MIN_ACCEPTABLE_LED_VOLTAGE &&\r
- msg->config_led.voltage_orange < FMW_MAX_ACCEPTABLE_LED_VOLTAGE)) {\r
- return FMW_Result_Error_MessageHandler_Init_InvalidLEDVoltage;\r
- }\r
- if (!(msg->config_led.voltage_hysteresis >= FMW_MIN_ACCEPTABLE_LED_VOLTAGE &&\r
- msg->config_led.voltage_hysteresis < FMW_MAX_ACCEPTABLE_LED_VOLTAGE)) {\r
- return FMW_Result_Error_MessageHandler_Init_InvalidLEDVoltage;\r
- }\r
- if (!(msg->config_led.update_period >= FMW_MIN_ACCEPTABLE_LED_UPDATE_PERIOD &&\r
- msg->config_led.update_period < FMW_MAX_ACCEPTABLE_LED_UPDATE_PERIOD)) {\r
- return FMW_Result_Error_MessageHandler_Init_InvalidLEDVoltage;\r
+ if (!(msg->config_led.update_period > 0)) {\r
+ return FMW_Result_Error_MessageHandler_Init_NonPositiveLEDUpdatePeriod;\r
}\r
\r
pled.voltage_red = msg->config_led.voltage_red;\r
pled.voltage_hysteresis = msg->config_led.voltage_hysteresis;\r
led_update_period = msg->config_led.update_period;\r
} break;\r
+ case FMW_MessageType_Status: // NOTE(lb): allow status messages in init mode?\r
+ case FMW_MessageType_Velocity: {\r
+ return FMW_Result_Error_Command_NotAvailable;\r
+ } break;\r
+ default: {\r
+ return FMW_Result_Error_Command_NotRecognized;\r
+ } break;\r
+ }\r
+ } break;\r
+ case FMW_State_Running: {\r
+ switch (msg->header.type) {\r
+ case FMW_MessageType_Status: { // TODO(lb): this should be `GetStatus` or something like that.\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
+ (void)current_ticks_left;\r
+ (void)current_ticks_right;\r
+ // TODO(lb): add the rest.\r
+ } break;\r
+ case FMW_MessageType_Velocity: {\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
+ } break;\r
+ case FMW_MessageType_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
+ } break;\r
+ default: {\r
+ return FMW_Result_Error_Command_NotRecognized;\r
+ } break;\r
}\r
} break;\r
}\r
return FMW_Result_Ok;\r
}\r
\r
-int32_t fmw_result_format(char buffer[], size_t buffer_size,\r
- const char *filename, int16_t filename_length,\r
- int32_t line, FMW_Result result) {\r
- static const char* const variant_strings[] = {\r
- #define X(Variant) #Variant,\r
- FMW_RESULT_VARIANTS(X)\r
- #undef X\r
- };\r
-\r
- return snprintf(buffer, buffer_size,\r
- "[%.*s:%ld] failed with result: %s\r\n",\r
- filename_length, filename, line,\r
- variant_strings[result]);\r
-}\r
-\r
-void fmw_result_log_uart(UART_HandleTypeDef *huart, FMW_Result result,\r
- const char *filename, int16_t filename_length,\r
- int32_t line) {\r
- if (fmw_state != FMW_State_Init) {\r
- fmw_motor_brake(&motors.left);\r
- fmw_motor_brake(&motors.right);\r
- }\r
-\r
- char buff[512] = {0};\r
- int32_t length = fmw_result_format(buff, sizeof buff, filename, filename_length, line, result);\r
- HAL_UART_Transmit(huart, (uint8_t*)buff, length, HAL_MAX_DELAY);\r
-}\r
-\r
// TODO(lb): move to fmw. maybe create a FMW_Buzzer?\r
void buzzer_set(bool on) {\r
if (on) {\r
\r
// TIMER 100Hz PID control\r
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {\r
- // NOTE(lb): for transmission\r
+ // NOTE(lb): metrics taken for transmission\r
ticks_left += fmw_encoder_count_get(&encoders.left);\r
ticks_right += fmw_encoder_count_get(&encoders.right);\r
\r
}\r
}\r
\r
-#if 0\r
-void HAL_UART_RxCpltCallback(UART_HandleTypeDef *UartHandle) {\r
- /*\r
- * Manage received message\r
- */\r
-\r
- uint32_t crc_rx = HAL_CRC_Calculate(&hcrc, (uint32_t*) &vel_msg, 8);\r
-\r
- float linear_velocity;\r
- float angular_velocity;\r
-\r
- if (crc_rx == vel_msg.crc) {\r
- linear_velocity = vel_msg.linear_velocity;\r
- angular_velocity = vel_msg.angular_velocity;\r
- otto_status = MessageStatusCode_Running;\r
- } else {\r
- linear_velocity = 0;\r
- angular_velocity = 0;\r
- otto_status = MessageStatusCode_Error_Velocity;\r
- }\r
-\r
- odometry_setpoint_from_cmdvel(&odom, linear_velocity, angular_velocity);\r
- float left_setpoint = odom.velocity.left;\r
- float right_setpoint = odom.velocity.right;\r
-\r
- pid_left.setpoint = left_setpoint;\r
- pid_right.setpoint = right_setpoint;\r
+void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) {\r
+ if (huart == UART_MESSANGER_HANDLE) {\r
+ // NOTE(lb): i don't think another interrupt of this kind can occur\r
+ // while i'm still handling the previous one so casting away\r
+ // volatile should be fine.\r
+ FMW_Result res = message_handler((FMW_Message*)&run_msg);\r
+ if (res != FMW_Result_Ok) { FMW_RESULT_LOG_UART(huart, res); }\r
\r
- float cross_setpoint = left_setpoint - right_setpoint;\r
- pid_cross.setpoint = cross_setpoint;\r
+ // NOTE(lb): listen for the next message.\r
+ HAL_UART_Receive_DMA(huart, (uint8_t*)&run_msg, sizeof run_msg);\r
\r
- /*\r
- * Manage new transmission\r
- */\r
+#if 0\r
+ /*\r
+ * Manage new transmission\r
+ */\r
\r
- int32_t left_ticks_tx = left_ticks + encoder_count_get(&encoders.left);\r
- int32_t right_ticks_tx = right_ticks + encoder_count_get(&encoders.right);\r
+ int32_t left_ticks_tx = left_ticks + encoder_count_get(&encoders.left);\r
+ int32_t right_ticks_tx = right_ticks + encoder_count_get(&encoders.right);\r
\r
- status_msg.left_ticks = left_ticks_tx;\r
- status_msg.right_ticks = right_ticks_tx;\r
+ status_msg.left_ticks = left_ticks_tx;\r
+ status_msg.right_ticks = right_ticks_tx;\r
\r
- left_ticks = 0;\r
- right_ticks = 0;\r
+ left_ticks = 0;\r
+ right_ticks = 0;\r
\r
- float current_tx_millis = HAL_GetTick();\r
- status_msg.delta_millis = current_tx_millis - previous_tx_millis;\r
- previous_tx_millis = current_tx_millis;\r
+ float current_tx_millis = HAL_GetTick();\r
+ status_msg.delta_millis = current_tx_millis - previous_tx_millis;\r
+ previous_tx_millis = current_tx_millis;\r
\r
- status_msg.status = otto_status;\r
+ status_msg.status = otto_status;\r
\r
- uint32_t crc_tx = HAL_CRC_Calculate(&hcrc, (uint32_t*) &status_msg, 12);\r
+ uint32_t crc_tx = HAL_CRC_Calculate(&hcrc, (uint32_t*) &status_msg, 12);\r
\r
- status_msg.crc = crc_tx;\r
+ status_msg.crc = crc_tx;\r
\r
- if (tx_done_flag) {\r
- HAL_UART_Transmit_DMA(&huart6, (uint8_t*) &status_msg, sizeof(status_msg));\r
- tx_done_flag = 0;\r
+ if (tx_done_flag) {\r
+ HAL_UART_Transmit_DMA(&huart6, (uint8_t*) &status_msg, sizeof(status_msg));\r
+ tx_done_flag = 0;\r
+ }\r
+#endif\r
}\r
-\r
- HAL_UART_Receive_DMA(&huart6, (uint8_t*) &vel_msg, sizeof(vel_msg));\r
}\r
-#endif\r
-\r
\r
void HAL_UART_TxCpltCallback(UART_HandleTypeDef *UartHandle) {\r
tx_done_flag = 1;\r
projects / pioneer-stm32 / commitdiff