Add parameters to specify maximum allowd time jump (#202)

openspur · Apr 22, 2024 · ebd19cf · ebd19cf
1 parent 8c29b1d
commit ebd19cf
Show file tree

Hide file tree

Showing 3 changed files with 39 additions and 1 deletion.
diff --git a/include/ypparam.h b/include/ypparam.h
@@ -218,6 +218,8 @@ typedef enum
   YP_PARAM_INDEX_GEAR,
 
   YP_PARAM_DEVICE_TIMEOUT,
+  YP_PARAM_MAX_TIME_JUMP,
+  YP_PARAM_MAX_TIME_JUMP_NEG,
 
   YP_PARAM_NUM  ///< パラメータの最大値
 } YPSpur_param;
@@ -296,6 +298,8 @@ typedef enum
         "INDEX_FALL_ANGLE",    \
         "INDEX_GEAR",          \
         "DEVICE_TIMEOUT",      \
+        "MAX_TIME_JUMP",       \
+        "-MAX_TIME_JUMP",      \
   }
 
 #define YP_PARAM_NECESSARY \
@@ -371,6 +375,8 @@ typedef enum
         0,                 \
         0,                 \
         0,                 \
+        0,                 \
+        0,                 \
   }
 
 #define YP_PARAM_COMMENT                                                                        \
@@ -447,6 +453,8 @@ typedef enum
         "[rad] Index signal falling edge angle at CW rotation",                                 \
         "[in/out] Index signal gear ratio",                                                     \
         "[s] Timeout of the communication with the device",                                     \
+        "[s] Maximum allowed positive system time jump, used for --exit-on-time-jump",          \
+        "[s] Maximum allowed negative system time jump, used for --exit-on-time-jump",          \
   }
 
 enum motor_id

diff --git a/src/control_vehicle.c b/src/control_vehicle.c
@@ -560,7 +560,7 @@ void control_loop(void)
       const double dt = now - last_time;
       const double dt_error = dt - expected_dt;
       last_time = now;
-      if (dt_error < -control_cycle || control_cycle < dt_error)
+      if (dt_error < -p(YP_PARAM_MAX_TIME_JUMP_NEG, 0) || p(YP_PARAM_MAX_TIME_JUMP, 0) < dt_error)
       {
         yprintf(OUTPUT_LV_ERROR, "Detected system time jump: %0.5fs\n", dt_error);
         static int status = EXIT_FAILURE;

diff --git a/src/param.c b/src/param.c
@@ -940,6 +940,36 @@ int set_paramptr(FILE* paramfile)
     g_P_changed[YP_PARAM_TORQUE_UNIT][j] = ischanged_p(YP_PARAM_TORQUE_FINENESS, j);
   }
 
+  if (g_P_set[YP_PARAM_MAX_TIME_JUMP][0])
+  {
+    if (g_P[YP_PARAM_MAX_TIME_JUMP][0] <= 0)
+    {
+      yprintf(OUTPUT_LV_ERROR, "ERROR: MAX_TIME_JUMP must be > 0.0.\n");
+      param_error = 1;
+    }
+  }
+  else
+  {
+    g_P[YP_PARAM_MAX_TIME_JUMP][0] = g_P[YP_PARAM_CYCLE][0];
+  }
+  if (g_P_set[YP_PARAM_MAX_TIME_JUMP_NEG][0])
+  {
+    if (g_P[YP_PARAM_MAX_TIME_JUMP_NEG][0] <= 0)
+    {
+      yprintf(OUTPUT_LV_ERROR, "ERROR: -MAX_TIME_JUMP must be > 0.0.\n");
+      param_error = 1;
+    }
+  }
+  else
+  {
+    g_P[YP_PARAM_MAX_TIME_JUMP_NEG][0] = g_P[YP_PARAM_CYCLE][0];
+  }
+
+  if (param_error)
+  {
+    return 0;
+  }
+
   // パラメータの指定によって自動的に求まるパラメータの計算
   calc_param_inertia2ff();