Add option to stop control on system time jump

include/param.h

@@ -52,6 +52,7 @@ typedef enum
   OPTION_UPDATE_PARAM = 0x40000,
   OPTION_HIGH_PREC = 0x80000,
   OPTION_PING = 0x100000,
+  OPTION_EXIT_ON_TIME_JUMP = 0x200000,
 } ParamOptions;
 
 #define OPTION_DEFAULT (OPTION_HIGH_PREC)

src/control_vehicle.c

@@ -454,6 +454,17 @@ double gravity_compensation(OdometryPtr odm, SpurUserParamsPtr spur)
 
 void control_loop_cleanup(void* data)
 {
+  int i;
+  ParametersPtr param = get_param_ptr();
+
+  for (i = 0; i < YP_PARAM_MAX_MOTOR_NUM; i++)
+  {
+    if (param->motor_enable[i])
+    {
+      parameter_set(PARAM_servo, i, SERVO_LEVEL_STOP);
+    }
+  }
+
   yprintf(OUTPUT_LV_INFO, "Trajectory control loop stopped.\n");
 }
 
@@ -503,13 +514,16 @@ void control_loop(void)
   yprintf(OUTPUT_LV_INFO, "Trajectory control loop started.\n");
   pthread_cleanup_push(control_loop_cleanup, NULL);
 
+  double last_time = get_time();
+
 #if defined(HAVE_CLOCK_NANOSLEEP)  // clock_nanosleepが利用可能
   struct timespec request;
 
   if (clock_gettime(CLOCK_MONOTONIC, &request) == -1)
   {
-    yprintf(OUTPUT_LV_ERROR, "error on clock_gettime\n");
-    exit(0);
+    yprintf(OUTPUT_LV_ERROR, "Error on clock_gettime\n");
+    static int status = EXIT_FAILURE;
+    pthread_exit(&status);
   }
 #endif  // defined(HAVE_CLOCK_NANOSLEEP)
   while (1)
@@ -524,6 +538,21 @@ void control_loop(void)
     yp_usleep(p(YP_PARAM_CONTROL_CYCLE, 0) * 1000000);
 #endif  // defined(HAVE_CLOCK_NANOSLEEP)
 
+    if ((option(OPTION_EXIT_ON_TIME_JUMP)))
+    {
+      const double now = get_time();
+      const double dt = now - last_time;
+      const double expected_dt = p(YP_PARAM_CONTROL_CYCLE, 0);
+      const double dt_error = dt - expected_dt;
+      last_time = now;
+      if (dt_error < -expected_dt || expected_dt < dt_error)
+      {
+        yprintf(OUTPUT_LV_ERROR, "Detected system time jump: %0.5fs\n", dt_error);
+        static int status = EXIT_FAILURE;
+        pthread_exit(&status);
+      }
+    }
+
     coordinate_synchronize(odometry, spur);
     run_control(*odometry, spur);
 

src/param.c

@@ -153,6 +153,7 @@ void arg_longhelp(int argc, char* argv[])
   fprintf(stderr, "  --socket <port>          Use socket ipc.\n");
   fprintf(stderr, "  --daemon                 Run in daemon mode.\n");
   fprintf(stderr, "  --ping                   Ping RS485 chained devices.\n");
+  fprintf(stderr, "  --exit-on-time-jump      Immediately stop control and exit on system time jump.\n");
 }
 
 /* 引数の説明 */
@@ -364,6 +365,10 @@ int arg_analyze(int argc, char* argv[])
         break;
       }
     }
+    else if (!strcmp(argv[i], "--exit-on-time-jump"))
+    {
+      g_param.option |= OPTION_EXIT_ON_TIME_JUMP;
+    }
     else
     {
       yprintf(OUTPUT_LV_ERROR, "ERROR : invalid option -- '%s'.\n", argv[i]);

src/ypspur-coordinator.c

@@ -226,6 +226,7 @@ int main(int argc, char* argv[])
 
   fflush(stderr);
 
+  int* control_thread_status = NULL;
   command_thread_en = 0;
   command_thread_en = 0;
   do
@@ -526,8 +527,10 @@ int main(int argc, char* argv[])
       }
       else
       {
-        while (1)
-          yp_usleep(1000000);
+        // Clear control thread enable flag to avoid multiple join on signal exit.
+        control_thread_en = 0;
+        // Wait control thread instead of odometry receive loop on simuation mode.
+        pthread_join(control_thread, (void**)&control_thread_status);
       }
       yprintf(OUTPUT_LV_INFO, "Connection to %s was closed.\n", param->device_name);
     }
@@ -543,7 +546,7 @@ int main(int argc, char* argv[])
     if (control_thread_en)
     {
       pthread_cancel(control_thread);
-      pthread_join(control_thread, NULL);
+      pthread_join(control_thread, (void**)&control_thread_status);
       control_thread_en = 0;
     }
     if (command_thread_en)
@@ -553,7 +556,7 @@ int main(int argc, char* argv[])
       command_thread_en = 0;
     }
 
-    if (option(OPTION_RECONNECT) && quit == 0)
+    if (option(OPTION_RECONNECT) && quit == 0 && control_thread_status == NULL)
     {
       init_spur_command();
       yp_usleep(500000);
@@ -585,5 +588,10 @@ int main(int argc, char* argv[])
   yp_usleep(200000);
   fflush(stderr);
 
+  if (control_thread_status != NULL && control_thread_status != PTHREAD_CANCELED)
+  {
+    return *control_thread_status;
+  }
+
   return (quit ? EXIT_SUCCESS : EXIT_FAILURE);
 }