diff --git a/include/ypparam.h b/include/ypparam.h index d000608..c36703c 100644 --- 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 index a236284..cc0c2d1 100644 --- 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 index 3946ce2..54bc7ba 100644 --- 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();