Show driver error only if the state is changed (#122)

* Show driver error only if the state is changed * Show new errors: EEPROM, INTERNAL
openspur · Dec 10, 2019 · 639ade4 · 639ade4
1 parent 590dbef
commit 639ade4
Show file tree

Hide file tree

Showing 2 changed files with 29 additions and 14 deletions.
diff --git a/include/shvel-param.h b/include/shvel-param.h
@@ -104,7 +104,9 @@ typedef enum
   ERROR_LOW_VOLTAGE = 0x0001,
   ERROR_HALL_SEQ = 0x0002,
   ERROR_HALL_ENC = 0x0004,
-  ERROR_WATCHDOG = 0x0008
+  ERROR_WATCHDOG = 0x0008,
+  ERROR_EEPROM = 0x0010,
+  ERROR_INTERNAL = 0x0020,
 } YPSpur_shvel_error_state;
 
 #endif  // SHVEL_PARAM_H
diff --git a/src/odometry.c b/src/odometry.c
@@ -289,21 +289,34 @@ void process_int(
     }
     case INT_error_state:
     {
+      if (err->state[id] != value)
+      {
+        if (value == ERROR_NONE)
+        {
+          yprintf(OUTPUT_LV_INFO, "Info: The driver of motor_id %d got back to normal\n", id);
+        }
+        else
+        {
+          yprintf(OUTPUT_LV_ERROR, "Error: The driver of motor_id %d returned ", id);
+
+          if (value & ERROR_LOW_VOLTAGE)
+            yprintf(OUTPUT_LV_ERROR, "ERROR_LOW_VOLTAGE ");
+          if (value & ERROR_HALL_SEQ)
+            yprintf(OUTPUT_LV_ERROR, "ERROR_HALL_SEQ ");
+          if (value & ERROR_HALL_ENC)
+            yprintf(OUTPUT_LV_ERROR, "ERROR_HALL_ENC ");
+          if (value & ERROR_WATCHDOG)
+            yprintf(OUTPUT_LV_ERROR, "ERROR_WATCHDOG ");
+          if (value & ERROR_EEPROM)
+            yprintf(OUTPUT_LV_ERROR, "ERROR_EEPROM ");
+          if (value & ERROR_INTERNAL)
+            yprintf(OUTPUT_LV_ERROR, "ERROR_INTERNAL ");
+
+          yprintf(OUTPUT_LV_ERROR, "\n");
+        }
+      }
       err->state[id] = value;
       err->time[id] = receive_time;
-      if (value != ERROR_NONE)
-        yprintf(OUTPUT_LV_ERROR, "Error: The driver of motor_id %d returned ", id);
-      if (value & ERROR_LOW_VOLTAGE)
-        yprintf(OUTPUT_LV_ERROR, "ERROR_LOW_VOLTAGE ");
-      if (value & ERROR_HALL_SEQ)
-        yprintf(OUTPUT_LV_ERROR, "ERROR_HALL_SEQ ");
-      if (value & ERROR_HALL_ENC)
-        yprintf(OUTPUT_LV_ERROR, "ERROR_HALL_ENC ");
-      if (value & ERROR_WATCHDOG)
-        yprintf(OUTPUT_LV_ERROR, "ERROR_WATCHDOG ");
-
-      if (value != ERROR_NONE)
-        yprintf(OUTPUT_LV_ERROR, "\n");
       break;
     }
     default: