From 21ae309536c5d444443c34b4ed8bac8e75c2631c Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Thu, 28 Mar 2024 14:26:14 +0900 Subject: [PATCH 01/14] Add option to exit on system time jump --- include/param.h | 1 + include/utility.h | 1 + src/control_vehicle.c | 47 +++++++++++++++++++++++++++++-------------- src/param.c | 5 +++++ src/utility.c | 11 +++++++++- 5 files changed, 49 insertions(+), 16 deletions(-) diff --git a/include/param.h b/include/param.h index cc188cd..3cde613 100644 --- a/include/param.h +++ b/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) diff --git a/include/utility.h b/include/utility.h index 9dd3c91..6fdbce9 100644 --- a/include/utility.h +++ b/include/utility.h @@ -22,6 +22,7 @@ #define UTILITY_H double get_time(void); +double get_monotonic_time(void); void yp_usleep(int usec); void set_sigint_handler(void (*handler)(int)); void hook_pre_global(void); diff --git a/src/control_vehicle.c b/src/control_vehicle.c index 7a46ad3..63ac5ca 100644 --- a/src/control_vehicle.c +++ b/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,6 +514,8 @@ 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_LIBRT) // clock_nanosleepが利用可能 struct timespec request; @@ -511,31 +524,36 @@ void control_loop(void) yprintf(OUTPUT_LV_ERROR, "error on clock_gettime\n"); exit(0); } +#else + int request; + request = (p(YP_PARAM_CONTROL_CYCLE, 0) * 1000000); +#endif // defined(HAVE_LIBRT) + while (1) { +#if defined(HAVE_LIBRT) // clock_nanosleepが利用可能 request.tv_nsec += (p(YP_PARAM_CONTROL_CYCLE, 0) * 1000000000); request.tv_sec += request.tv_nsec / 1000000000; request.tv_nsec = request.tv_nsec % 1000000000; clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &request, 0); - coordinate_synchronize(odometry, spur); - run_control(*odometry, spur); +#else + yp_usleep(request); +#endif // defined(HAVE_LIBRT) - if ((option(OPTION_WITHOUT_DEVICE))) + if ((option(OPTION_EXIT_ON_TIME_JUMP))) { - simulate_control(*odometry, spur); + 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 = expected_dt - dt; + last_time = now; + if (dt_error < -expected_dt || expected_dt < dt_error) + { + break; + } } - // スレッドの停止要求チェック - pthread_testcancel(); - } -#else - int request; - request = (p(YP_PARAM_CONTROL_CYCLE, 0) * 1000000); - - while (1) - { - yp_usleep(request); coordinate_synchronize(odometry, spur); run_control(*odometry, spur); @@ -547,7 +565,6 @@ void control_loop(void) // スレッドの停止要求チェック pthread_testcancel(); } -#endif // defined(HAVE_LIBRT) pthread_cleanup_pop(1); } diff --git a/src/param.c b/src/param.c index a787914..3946ce2 100644 --- a/src/param.c +++ b/src/param.c @@ -153,6 +153,7 @@ void arg_longhelp(int argc, char* argv[]) fprintf(stderr, " --socket 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]); diff --git a/src/utility.c b/src/utility.c index 126da4d..93ec88f 100644 --- a/src/utility.c +++ b/src/utility.c @@ -48,7 +48,16 @@ double get_time(void) gettimeofday(¤t, NULL); - return current.tv_sec + current.tv_usec / 1000000.0; + return current.tv_sec + current.tv_usec / 1e6; +} + +double get_monotonic_time(void) +{ + struct timespec current; + + clock_gettime(CLOCK_MONOTONIC, ¤t); + + return current.tv_sec + current.tv_nsec / 1e9; } void yp_usleep(int usec) From 3829770085094c34359861011359e35873d5e052 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Thu, 28 Mar 2024 14:32:04 +0900 Subject: [PATCH 02/14] Remove unused func --- include/utility.h | 1 - src/utility.c | 11 +---------- 2 files changed, 1 insertion(+), 11 deletions(-) diff --git a/include/utility.h b/include/utility.h index 6fdbce9..9dd3c91 100644 --- a/include/utility.h +++ b/include/utility.h @@ -22,7 +22,6 @@ #define UTILITY_H double get_time(void); -double get_monotonic_time(void); void yp_usleep(int usec); void set_sigint_handler(void (*handler)(int)); void hook_pre_global(void); diff --git a/src/utility.c b/src/utility.c index 93ec88f..6654a33 100644 --- a/src/utility.c +++ b/src/utility.c @@ -48,16 +48,7 @@ double get_time(void) gettimeofday(¤t, NULL); - return current.tv_sec + current.tv_usec / 1e6; -} - -double get_monotonic_time(void) -{ - struct timespec current; - - clock_gettime(CLOCK_MONOTONIC, ¤t); - - return current.tv_sec + current.tv_nsec / 1e9; + return current.tv_sec + current.tv_usec / 1000000; } void yp_usleep(int usec) From a44a78bb679efb7bdce77718f2f14e9ea52798d5 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Thu, 28 Mar 2024 14:38:41 +0900 Subject: [PATCH 03/14] Add error log --- src/control_vehicle.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/control_vehicle.c b/src/control_vehicle.c index 63ac5ca..750b8e1 100644 --- a/src/control_vehicle.c +++ b/src/control_vehicle.c @@ -550,6 +550,7 @@ void control_loop(void) last_time = now; if (dt_error < -expected_dt || expected_dt < dt_error) { + yprintf(OUTPUT_LV_ERROR, "detected system time jump: %0.5f\n", dt_error); break; } } From 7e4f1aaf9d6c4b5e2d37a2617f6f6e628293556e Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Thu, 28 Mar 2024 15:15:52 +0900 Subject: [PATCH 04/14] Minor refactoring --- src/control_vehicle.c | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/src/control_vehicle.c b/src/control_vehicle.c index 750b8e1..4224939 100644 --- a/src/control_vehicle.c +++ b/src/control_vehicle.c @@ -514,21 +514,18 @@ 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_LIBRT) // clock_nanosleepが利用可能 struct timespec request; if (clock_gettime(CLOCK_MONOTONIC, &request) == -1) { yprintf(OUTPUT_LV_ERROR, "error on clock_gettime\n"); - exit(0); + exit(EXIT_FAILURE); } -#else - int request; - request = (p(YP_PARAM_CONTROL_CYCLE, 0) * 1000000); #endif // defined(HAVE_LIBRT) + double last_time = get_time(); + while (1) { #if defined(HAVE_LIBRT) // clock_nanosleepが利用可能 @@ -538,7 +535,7 @@ void control_loop(void) clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &request, 0); #else - yp_usleep(request); + yp_usleep(p(YP_PARAM_CONTROL_CYCLE, 0) * 1000000); #endif // defined(HAVE_LIBRT) if ((option(OPTION_EXIT_ON_TIME_JUMP))) From 297e7504cea982831edcec47e17b28313dac6df8 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Thu, 28 Mar 2024 15:43:54 +0900 Subject: [PATCH 05/14] Fix function existence checks --- CMakeLists.txt | 7 +++++++ src/control_vehicle.c | 6 +++--- 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 200cad8..750de5f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -67,6 +67,13 @@ endif(READLINE_FOUND) include(CheckFunctionExists) check_function_exists(longjmp HAVE_LONGJMP) check_function_exists(siglongjmp HAVE_SIGLONGJMP) +check_function_exists(clock_nanosleep HAVE_CLOCK_NANOSLEEP) + +add_compile_definitions( + HAVE_LONGJMP + HAVE_SIGLONGJMP + HAVE_CLOCK_NANOSLEEP +) configure_file(${CMAKE_CURRENT_SOURCE_DIR}/cmake/config.h.cmake ${CMAKE_CURRENT_BINARY_DIR}/include/config.h diff --git a/src/control_vehicle.c b/src/control_vehicle.c index 7a46ad3..40ecb2e 100644 --- a/src/control_vehicle.c +++ b/src/control_vehicle.c @@ -503,7 +503,7 @@ void control_loop(void) yprintf(OUTPUT_LV_INFO, "Trajectory control loop started.\n"); pthread_cleanup_push(control_loop_cleanup, NULL); -#if defined(HAVE_LIBRT) // clock_nanosleepが利用可能 +#if defined(HAVE_CLOCK_NANOSLEEP) // clock_nanosleepが利用可能 struct timespec request; if (clock_gettime(CLOCK_MONOTONIC, &request) == -1) @@ -523,7 +523,7 @@ void control_loop(void) if ((option(OPTION_WITHOUT_DEVICE))) { - simulate_control(*odometry, spur); + simulate_control(odometry, spur); } // スレッドの停止要求チェック @@ -547,7 +547,7 @@ void control_loop(void) // スレッドの停止要求チェック pthread_testcancel(); } -#endif // defined(HAVE_LIBRT) +#endif // defined(HAVE_CLOCK_NANOSLEEP) pthread_cleanup_pop(1); } From 0e258f431d454e48d4fd5f00d8daff1f96875b86 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Thu, 28 Mar 2024 15:56:34 +0900 Subject: [PATCH 06/14] Fix defs --- CMakeLists.txt | 32 ++++++++++++++++++++++++++------ cmake/config.h.cmake | 6 ------ 2 files changed, 26 insertions(+), 12 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 750de5f..7adec48 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -69,12 +69,6 @@ check_function_exists(longjmp HAVE_LONGJMP) check_function_exists(siglongjmp HAVE_SIGLONGJMP) check_function_exists(clock_nanosleep HAVE_CLOCK_NANOSLEEP) -add_compile_definitions( - HAVE_LONGJMP - HAVE_SIGLONGJMP - HAVE_CLOCK_NANOSLEEP -) - configure_file(${CMAKE_CURRENT_SOURCE_DIR}/cmake/config.h.cmake ${CMAKE_CURRENT_BINARY_DIR}/include/config.h ) @@ -102,6 +96,7 @@ if(READLINE_FOUND) ) endif(READLINE_FOUND) + # Static link options option(ENABLE_ALL_STATIC "Static link libgcc and libstdc++." OFF) @@ -113,6 +108,31 @@ if(ENABLE_ALL_STATIC) endif(ENABLE_ALL_STATIC) +# Compile flags + +if(CMAKE_VERSION VERSION_LESS "3.12") + add_compile_definitions( + HAVE_LONGJMP + HAVE_SIGLONGJMP + HAVE_CLOCK_NANOSLEEP + HAVE_SSM + ) +else() + if(HAVE_LONGJMP) + add_definitions(-DHAVE_LONGJMP) + endif() + if(HAVE_SIGLONGJMP) + add_definitions(-DHAVE_SIGLONGJMP) + endif() + if(HAVE_CLOCK_NANOSLEEP) + add_definitions(-DHAVE_CLOCK_NANOSLEEP) + endif() + if(HAVE_SSM) + add_definitions(-DHAVE_SSM) + endif() +endif() + + # Add subdirectories add_subdirectory(auxlib) diff --git a/cmake/config.h.cmake b/cmake/config.h.cmake index ffed201..8c0d9eb 100644 --- a/cmake/config.h.cmake +++ b/cmake/config.h.cmake @@ -11,10 +11,4 @@ #cmakedefine YP_PARAMS_DIR "@YP_PARAMS_DIR@" -#cmakedefine HAVE_SSM @HAVE_SSM@ - -#cmakedefine HAVE_LONGJMP @HAVE_LONGJMP@ - -#cmakedefine HAVE_SIGLONGJMP @HAVE_SIGLONGJMP@ - #endif From f1e560408802f6235c3300c5c02a832117df6ccf Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Thu, 28 Mar 2024 15:58:54 +0900 Subject: [PATCH 07/14] Fix cmake version check --- CMakeLists.txt | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7adec48..7947140 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -111,13 +111,6 @@ endif(ENABLE_ALL_STATIC) # Compile flags if(CMAKE_VERSION VERSION_LESS "3.12") - add_compile_definitions( - HAVE_LONGJMP - HAVE_SIGLONGJMP - HAVE_CLOCK_NANOSLEEP - HAVE_SSM - ) -else() if(HAVE_LONGJMP) add_definitions(-DHAVE_LONGJMP) endif() @@ -130,6 +123,13 @@ else() if(HAVE_SSM) add_definitions(-DHAVE_SSM) endif() +else() + add_compile_definitions( + HAVE_LONGJMP + HAVE_SIGLONGJMP + HAVE_CLOCK_NANOSLEEP + HAVE_SSM + ) endif() From ffe44ebee1ee3686b432141dd0b52f2a8aa46115 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Thu, 28 Mar 2024 16:02:11 +0900 Subject: [PATCH 08/14] Minor refactoring --- src/control_vehicle.c | 21 ++++----------------- 1 file changed, 4 insertions(+), 17 deletions(-) diff --git a/src/control_vehicle.c b/src/control_vehicle.c index 40ecb2e..c0b2657 100644 --- a/src/control_vehicle.c +++ b/src/control_vehicle.c @@ -511,31 +511,19 @@ void control_loop(void) yprintf(OUTPUT_LV_ERROR, "error on clock_gettime\n"); exit(0); } +#endif // defined(HAVE_CLOCK_NANOSLEEP) while (1) { +#if defined(HAVE_CLOCK_NANOSLEEP) // clock_nanosleepが利用可能 request.tv_nsec += (p(YP_PARAM_CONTROL_CYCLE, 0) * 1000000000); request.tv_sec += request.tv_nsec / 1000000000; request.tv_nsec = request.tv_nsec % 1000000000; clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &request, 0); - coordinate_synchronize(odometry, spur); - run_control(*odometry, spur); - - if ((option(OPTION_WITHOUT_DEVICE))) - { - simulate_control(odometry, spur); - } - - // スレッドの停止要求チェック - pthread_testcancel(); - } #else - int request; - request = (p(YP_PARAM_CONTROL_CYCLE, 0) * 1000000); + yp_usleep(p(YP_PARAM_CONTROL_CYCLE, 0) * 1000000); +#endif // defined(HAVE_CLOCK_NANOSLEEP) - while (1) - { - yp_usleep(request); coordinate_synchronize(odometry, spur); run_control(*odometry, spur); @@ -547,7 +535,6 @@ void control_loop(void) // スレッドの停止要求チェック pthread_testcancel(); } -#endif // defined(HAVE_CLOCK_NANOSLEEP) pthread_cleanup_pop(1); } From bd5e3b626fdfaa747590e9d49a3b97eb77ae33bf Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Thu, 28 Mar 2024 16:04:52 +0900 Subject: [PATCH 09/14] Fix add_compile_definitions condition --- CMakeLists.txt | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7947140..a8c596e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -124,12 +124,18 @@ if(CMAKE_VERSION VERSION_LESS "3.12") add_definitions(-DHAVE_SSM) endif() else() - add_compile_definitions( - HAVE_LONGJMP - HAVE_SIGLONGJMP - HAVE_CLOCK_NANOSLEEP - HAVE_SSM - ) + if(HAVE_LONGJMP) + add_compile_definitions(HAVE_LONGJMP) + endif() + if(HAVE_SIGLONGJMP) + add_compile_definitions(HAVE_SIGLONGJMP) + endif() + if(HAVE_CLOCK_NANOSLEEP) + add_compile_definitions(HAVE_CLOCK_NANOSLEEP) + endif() + if(HAVE_SSM) + add_compile_definitions(HAVE_SSM) + endif() endif() From 7e8afcf5abf4da5b6a286169231e3931bbaf4c8b Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Thu, 28 Mar 2024 16:21:31 +0900 Subject: [PATCH 10/14] Update error log --- src/control_vehicle.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/control_vehicle.c b/src/control_vehicle.c index 7077c49..45a4594 100644 --- a/src/control_vehicle.c +++ b/src/control_vehicle.c @@ -546,7 +546,7 @@ void control_loop(void) last_time = now; if (dt_error < -expected_dt || expected_dt < dt_error) { - yprintf(OUTPUT_LV_ERROR, "detected system time jump: %0.5f\n", dt_error); + yprintf(OUTPUT_LV_ERROR, "detected system time jump: %0.5fs\n", dt_error); break; } } From fb455fb8821b3aa71e25e67165dd5431884880e8 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Thu, 28 Mar 2024 16:53:10 +0900 Subject: [PATCH 11/14] Handle thread exit status --- src/control_vehicle.c | 10 ++++++---- src/ypspur-coordinator.c | 15 +++++++++++---- 2 files changed, 17 insertions(+), 8 deletions(-) diff --git a/src/control_vehicle.c b/src/control_vehicle.c index 45a4594..8674cf8 100644 --- a/src/control_vehicle.c +++ b/src/control_vehicle.c @@ -521,8 +521,9 @@ void control_loop(void) 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) @@ -546,8 +547,9 @@ void control_loop(void) 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); - break; + yprintf(OUTPUT_LV_ERROR, "Detected system time jump: %0.5fs\n", dt_error); + static int status = EXIT_FAILURE; + pthread_exit(&status); } } diff --git a/src/ypspur-coordinator.c b/src/ypspur-coordinator.c index c0698c8..0771135 100644 --- a/src/ypspur-coordinator.c +++ b/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,9 @@ int main(int argc, char* argv[]) } else { - while (1) - yp_usleep(1000000); + // Wait control thread instead of odometry receive loop on simuation mode. + pthread_join(control_thread, (void**)&control_thread_status); + control_thread_en = 0; } yprintf(OUTPUT_LV_INFO, "Connection to %s was closed.\n", param->device_name); } @@ -543,7 +545,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 +555,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 +587,10 @@ int main(int argc, char* argv[]) yp_usleep(200000); fflush(stderr); + if (control_thread_status != NULL) + { + return *control_thread_status; + } + return (quit ? EXIT_SUCCESS : EXIT_FAILURE); } From af7e9a70e3d0d49333d7515b65a7d7ef07a80ece Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Thu, 28 Mar 2024 16:56:03 +0900 Subject: [PATCH 12/14] Fix segfault on signal exit --- src/ypspur-coordinator.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/ypspur-coordinator.c b/src/ypspur-coordinator.c index 0771135..b1f4420 100644 --- a/src/ypspur-coordinator.c +++ b/src/ypspur-coordinator.c @@ -527,9 +527,10 @@ int main(int argc, char* argv[]) } else { + // 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); - control_thread_en = 0; } yprintf(OUTPUT_LV_INFO, "Connection to %s was closed.\n", param->device_name); } From 0f7d090f7075873e58553a6f44fcb4d5ed452a6a Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Fri, 29 Mar 2024 16:03:35 +0900 Subject: [PATCH 13/14] Fix status handling on stop by interrupt --- src/ypspur-coordinator.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ypspur-coordinator.c b/src/ypspur-coordinator.c index b1f4420..faa4c63 100644 --- a/src/ypspur-coordinator.c +++ b/src/ypspur-coordinator.c @@ -588,7 +588,7 @@ int main(int argc, char* argv[]) yp_usleep(200000); fflush(stderr); - if (control_thread_status != NULL) + if (control_thread_status != NULL && control_thread_status != PTHREAD_CANCELED) { return *control_thread_status; } From 8c51cd04794a2c99c395ed5bb5ceb8937e728950 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Fri, 29 Mar 2024 16:05:53 +0900 Subject: [PATCH 14/14] Fix sign of time error --- src/control_vehicle.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/control_vehicle.c b/src/control_vehicle.c index f342189..d35ba58 100644 --- a/src/control_vehicle.c +++ b/src/control_vehicle.c @@ -543,7 +543,7 @@ void control_loop(void) 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 = expected_dt - dt; + const double dt_error = dt - expected_dt; last_time = now; if (dt_error < -expected_dt || expected_dt < dt_error) {