Add DEVICE_TIMEOUT parameter and show packet lost error

include/ypparam.h

@@ -206,6 +206,8 @@ typedef enum
   YP_PARAM_INDEX_FALL_ANGLE,
   YP_PARAM_INDEX_GEAR,
 
+  YP_PARAM_DEVICE_TIMEOUT,
+
   YP_PARAM_NUM  ///< パラメータの最大値
 } YPSpur_param;
 
@@ -281,6 +283,7 @@ typedef enum
         "INDEX_RISE_ANGLE",    \
         "INDEX_FALL_ANGLE",    \
         "INDEX_GEAR",          \
+        "DEVICE_TIMEOUT",      \
   }
 
 #define YP_PARAM_NECESSARY \
@@ -354,6 +357,7 @@ typedef enum
         0,                 \
         0,                 \
         0,                 \
+        0,                 \
   }
 
 #define YP_PARAM_COMMENT                                                                        \
@@ -428,6 +432,7 @@ typedef enum
         "[rad] Index signal rising edge angle at CW rotation",                                  \
         "[rad] Index signal falling edge angle at CW rotation",                                 \
         "[in/out] Index signal gear ratio",                                                     \
+        "[s] Timeout of the communication with the device",                                     \
   }
 
 enum motor_id

src/odometry.c

@@ -420,8 +420,11 @@ double time_synchronize(double receive_time, int readnum, int wp)
   const int lost = lround(error / g_interval);
   if (lost != 0)
   {
-    if (option(OPTION_SHOW_TIMESTAMP))
-      printf("%d packet(s) might be lost!\n", lost);
+    if (abs(lost) > 1 || option(OPTION_SHOW_TIMESTAMP))
+    {
+      // lost=+1/-1 is a jitter in most case. Show abs(lost)>1 as an error.
+      yprintf(OUTPUT_LV_ERROR, "%d packets might be lost!\n", lost);
+    }
     error -= lost * g_interval;
     g_offset_point -= lost;
   }

src/param.c

@@ -903,6 +903,12 @@ int set_paramptr(FILE *paramfile)
         g_P_changed[YP_PARAM_ENCODER_DENOMINATOR][j] = 1;
       g_P[YP_PARAM_ENCODER_DENOMINATOR][j] = 1.0;
     }
+    if (!g_P_set[YP_PARAM_DEVICE_TIMEOUT][j])
+    {
+      // YP_PARAM_DEVICE_TIMEOUT is always sent on reloading.
+      // g_P_changed is not needed to be marked.
+      g_P[YP_PARAM_DEVICE_TIMEOUT][j] = 0.3;
+    }
 
     // Process internally calculated parameters
     g_P[YP_PARAM_TORQUE_UNIT][j] = 1.0 / g_P[YP_PARAM_TORQUE_FINENESS][j];
@@ -1511,7 +1517,8 @@ int set_param_velocity(void)
   {
     if (!g_param.motor_enable[j])
       continue;
-    parameter_set(PARAM_watch_dog_limit, j, 300);
+    // [s] -> [ms]
+    parameter_set(PARAM_watch_dog_limit, j, g_P[YP_PARAM_DEVICE_TIMEOUT][j] * 1000);
   }
   return 1;
 }

src/serial.c

@@ -445,8 +445,15 @@ int serial_recieve(int (*serial_event)(char *, int, double, void *), void *data)
     struct timeval tv;
     size_t len;
 
-    tv.tv_sec = 0;
-    tv.tv_usec = 200000;
+    // [s] -> [us]
+    long long int timeout_us = p(YP_PARAM_DEVICE_TIMEOUT, 0) * 1000000;
+    if (timeout_us == 0)
+    {
+      // Default timeout before loading parameter file.
+      timeout_us = 200000;
+    }
+    tv.tv_sec = timeout_us / 1000000;
+    tv.tv_usec = timeout_us % 1000000;
     FD_ZERO(&rfds);
     FD_SET(g_device_port, &rfds);