error checks in message received interrupt

diff --git a/pioneer_controller/Core/Src/firmware/fwm_core.c b/pioneer_controller/Core/Src/firmware/fwm_core.c
index 0863b6a47f1979400c26a256a9835e602d7100a5..5e473e461a9da23677c433b596333d42ea20016c 100644 (file)
--- a/pioneer_controller/Core/Src/firmware/fwm_core.c
+++ b/pioneer_controller/Core/Src/firmware/fwm_core.c
@@ -275,6 +275,7 @@ FMW_Result fmw_encoders_update(void) {
 
 FMW_Result fmw_encoder_get_linear_velocity(const FMW_Encoder *encoder, float meters_traveled, float *linear_velocity) {
   if (encoder < fmw_state.encoders || encoder >= (fmw_state.encoders + fmw_state.encoders_count)) {
+    *linear_velocity = 0.f;
     return FMW_Result_Error_InvalidArguments;
   }
   float deltatime = encoder->current_millis - encoder->previous_millis;

diff --git a/pioneer_controller/Core/Src/main.c b/pioneer_controller/Core/Src/main.c
index 483423d512b8a25d2f7c35235053d8e1263543cc..fdafeeed0dcf5d9ae7eb591d9bbbc4174c3a1615 100644 (file)
--- a/pioneer_controller/Core/Src/main.c
+++ b/pioneer_controller/Core/Src/main.c
@@ -847,6 +847,8 @@ void start(void) {
   FMW_InitInfo fmw_info = {\r
     .motors = motors.values,\r
     .motors_count = ARRLENGTH(motors.values),\r
+    .encoders = encoders.values,\r
+    .encoders_count = ARRLENGTH(encoders.values),\r
     .emergency = {\r
       .timer = &htim7,\r
       .on_begin = emergency_mode_begin,\r
@@ -921,9 +923,9 @@ void message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) {
   case FMW_Mode_Config: {\r
     switch (msg->header.type) {\r
     case FMW_MessageType_ModeChange_Run: {\r
-      fmw_encoders_init();\r
-      fmw_motors_init();\r
-      fmw_led_init(&pled);\r
+      result = fmw_encoders_init(); if (result != FMW_Result_Ok) { goto msg_contains_error; }\r
+      result = fmw_motors_init();   if (result != FMW_Result_Ok) { goto msg_contains_error; }\r
+      result = fmw_led_init(&pled); if (result != FMW_Result_Ok) { goto msg_contains_error; }\r
 \r
       // Right and left motors have the same parameters\r
       pid_max = (int32_t)htim4.Instance->ARR;\r
@@ -934,7 +936,7 @@ void message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) {
       HAL_StatusTypeDef timer_status = HAL_TIM_Base_Start_IT(&htim6);\r
       FMW_ASSERT(timer_status == HAL_OK);\r
 \r
-      fmw_mode_transition(FMW_Mode_Run);\r
+      FMW_ASSERT(fmw_mode_transition(FMW_Mode_Run) == FMW_Result_Ok);\r
     } break;\r
     case FMW_MessageType_Config_Robot: {\r
       if (!(msg->config_robot.baseline > 0.f)) {\r
@@ -1004,8 +1006,8 @@ void message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) {
 \r
       int32_t ticks_measured_left = 0;\r
       int32_t ticks_measured_right = 0;\r
-      fmw_encoder_count_get(&encoders.left, &ticks_measured_left);\r
-      fmw_encoder_count_get(&encoders.right, &ticks_measured_right);\r
+      result = fmw_encoder_count_get(&encoders.left, &ticks_measured_left);   if (result != FMW_Result_Ok) { goto msg_contains_error; }\r
+      result = fmw_encoder_count_get(&encoders.right, &ticks_measured_right); if (result != FMW_Result_Ok) { goto msg_contains_error; }\r
 \r
       int32_t current_ticks_left = ticks_left + ticks_measured_left;\r
       int32_t current_ticks_right = ticks_right + ticks_measured_right;\r
@@ -1035,17 +1037,17 @@ void message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) {
     } break;\r
     case FMW_MessageType_ModeChange_Config: {\r
       fmw_motors_stop();\r
-      fmw_encoder_count_reset(&encoders.left);\r
-      fmw_encoder_count_reset(&encoders.right);\r
+      result = fmw_encoder_count_reset(&encoders.left);  if (result != FMW_Result_Ok) { goto msg_contains_error; }\r
+      result = fmw_encoder_count_reset(&encoders.right); if (result != FMW_Result_Ok) { goto msg_contains_error; }\r
 \r
-      fmw_encoders_deinit();\r
-      fmw_motors_deinit();\r
-      fmw_led_deinit(&pled);\r
+      result = fmw_encoders_deinit(); if (result != FMW_Result_Ok) { goto msg_contains_error; }\r
+      result = fmw_motors_deinit();   if (result != FMW_Result_Ok) { goto msg_contains_error; }\r
+      result = fmw_led_deinit(&pled); if (result != FMW_Result_Ok) { goto msg_contains_error; }\r
 \r
       HAL_StatusTypeDef timer_status = HAL_TIM_Base_Stop_IT(&htim6);\r
       FMW_ASSERT(timer_status == HAL_OK);\r
 \r
-      fmw_mode_transition(FMW_Mode_Config);\r
+      FMW_ASSERT(fmw_mode_transition(FMW_Mode_Config) == FMW_Result_Ok);\r
     } break;\r
     case FMW_MessageType_Emergency_Begin: {\r
       fmw_emergency_begin();\r