diff --git a/CMakeLists.txt b/CMakeLists.txt index c2f8598..ef98394 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -113,19 +113,6 @@ add_subdirectory(samples) if(GTEST_FOUND) enable_testing() - set(CTEST_ENVIRONMENT CTEST_OUTPUT_ON_FAILURE=1) - - include(CheckCXXCompilerFlag) - check_cxx_compiler_flag("-std=c++11" COMPILER_SUPPORTS_CXX11) - check_cxx_compiler_flag("-std=c++0x" COMPILER_SUPPORTS_CXX0X) - if(COMPILER_SUPPORTS_CXX11) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") - elseif(COMPILER_SUPPORTS_CXX0X) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") - else() - message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") - endif() - add_executable(param_load_test test/param_load.cpp src/adinput.c @@ -143,26 +130,8 @@ if(GTEST_FOUND) carte2d${TARGET_STATIC_POSTFIX} formula-calc${TARGET_STATIC_POSTFIX} ) - add_test(YPSpurParamTest param_load_test) - add_executable(time_synchronize_test - test/time_synchronize.cpp - src/adinput.c - src/communication.c - src/odometry.c - src/param.c - src/serial.c - src/ssm_spur_handler.c - src/utility.c - src/yprintf.c - ) - target_link_libraries(time_synchronize_test - ${GTEST_LIBRARIES} - ${THREAD_LIBRARIES} - carte2d${TARGET_STATIC_POSTFIX} - formula-calc${TARGET_STATIC_POSTFIX} - ) -add_test(YPSpurTimeSynchronize time_synchronize_test) + add_test(YPSpurParamTest param_load_test) endif(GTEST_FOUND) diff --git a/src/odometry.c b/src/odometry.c index eeecc04..f3975af 100644 --- a/src/odometry.c +++ b/src/odometry.c @@ -53,6 +53,7 @@ double g_interval; double g_offset; int g_offset_point; +double g_estimated_delay = 0; CSptr g_GL; CSptr g_SP; @@ -373,7 +374,7 @@ ErrorStatePtr get_error_state_ptr() */ double time_estimate(int readnum) { - return g_offset + g_interval * (double)(readnum - g_offset_point + 1); + return g_offset + g_interval * (double)(readnum - g_offset_point) + g_estimated_delay; } /** @@ -384,8 +385,9 @@ double time_estimate(int readnum) */ double time_synchronize(double receive_time, int readnum, int wp) { + static double prev_time = 0.0; + static int prev_readnum = 0; double measured_time; - static double prev_time; // 受信開始時刻を計算 if (SER_BAUDRATE != 0) @@ -400,17 +402,20 @@ double time_synchronize(double receive_time, int readnum, int wp) // (Currently all YP-Spur compatible devices uses USB as a communication interface.) measured_time -= 0.0005; - if ( fabs(g_offset - measured_time) > 0.5) + if (g_offset_point <= 0 || fabs(g_offset - measured_time) > 0.5) { // Reset if measured_time has too large error (500ms). + g_offset = measured_time; g_interval = SER_INTERVAL; - g_offset = measured_time - g_interval * readnum; + g_offset_point = readnum; + prev_time = measured_time; } // predict + g_offset += g_interval * (readnum - g_offset_point); g_offset_point = readnum; - g_offset += g_interval * readnum; + const double dt = measured_time - prev_time; double error = measured_time - g_offset; const int lost = lround(error / g_interval); @@ -421,34 +426,42 @@ double time_synchronize(double receive_time, int readnum, int wp) yprintf(OUTPUT_LV_WARNING, "%d packets might be lost!\n", lost); error -= lost * g_interval; - g_offset += lost * g_interval; + g_offset_point -= lost; } - else + else if (g_odometry.packet_lost_last != g_odometry.packet_lost) { - if (g_odometry.packet_lost_last != g_odometry.packet_lost) - { - // Discard lost=+1/-1 as a jitter. - if (abs(g_odometry.packet_lost_last - g_odometry.packet_lost) > 1) - yprintf(OUTPUT_LV_ERROR, "Error: total packet lost: %d\n", g_odometry.packet_lost); - g_odometry.packet_lost_last = g_odometry.packet_lost; - } + // Discard lost=+1/-1 as a jitter. + if (abs(g_odometry.packet_lost_last - g_odometry.packet_lost) > 1) + yprintf(OUTPUT_LV_ERROR, "Error: total packet lost: %d\n", g_odometry.packet_lost); + g_odometry.packet_lost_last = g_odometry.packet_lost; + } - const double dt = measured_time - prev_time; - if (fabs(dt - g_interval * readnum) < dt * 0.1) - { - const double interval_estim = dt / (double)readnum; - const double t_error = g_offset - measured_time; - g_interval = 0.99 * g_interval + 0.01 * interval_estim; - g_offset -= 0.1 * t_error; - } + static double error_integ = 0; + error_integ += error * dt; + + if (error < g_estimated_delay && lost == 0) + { + if (option(OPTION_SHOW_TIMESTAMP)) + printf("[update min_delay] delay: %0.3f\n", + error * 1000.0); + g_estimated_delay = g_estimated_delay * 0.5 + error * 0.5; } + g_estimated_delay *= (1.0 - dt / 120.0); + + g_offset += error * 0.2 + error_integ * 0.1; + + // aprox. 0.5 sec exp filter + g_interval = + 0.99 * g_interval + + 0.01 * ((measured_time - prev_time) / (double)(readnum - prev_readnum)); static int cnt_show_timestamp = 0; if (option(OPTION_SHOW_TIMESTAMP) && ++cnt_show_timestamp % 100 == 0) - printf("epoch: %0.8f, interval: %0.3f, delay: %0.3f\n", - g_offset, g_interval * 1000.0, error * 1000.0); + printf("epoch: %0.8f, interval: %0.3f, delay: %0.3f, min_delay: %0.3f\n", + g_offset, g_interval * 1000.0, error * 1000.0, g_estimated_delay * 1000.0); prev_time = measured_time; + prev_readnum = readnum; return g_offset; } diff --git a/test/time_synchronize.cpp b/test/time_synchronize.cpp deleted file mode 100644 index 77f4479..0000000 --- a/test/time_synchronize.cpp +++ /dev/null @@ -1,154 +0,0 @@ -// Copyright (c) 2021 The YP-Spur Authors, except where otherwise indicated. -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to -// deal in the Software without restriction, including without limitation the -// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or -// sell copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include -#include - -#include - -#include - -#ifdef __cplusplus -extern "C" { -#endif // __cplusplus - -#include -#include -#include - -extern double g_interval; -extern double g_offset; -extern Odometry g_odometry; -extern Parameters g_param; - -double time_synchronize(double receive_time, int readnum, int wp); - -#ifdef __cplusplus -} -#endif // __cplusplus - -TEST(TimeSynchronize, PacketLost) -{ - g_param.output_lv = OUTPUT_LV_DEBUG; - g_param.option = OPTION_SHOW_TIMESTAMP; - g_interval = SER_INTERVAL; - g_offset = 0; - g_odometry.packet_lost = 0; - - time_synchronize(10.005, 1, 0); - EXPECT_EQ(g_odometry.packet_lost, 0); - EXPECT_NEAR(time_estimate(0), 10.005, 0.001); - - time_synchronize(10.010, 1, 0); - EXPECT_EQ(g_odometry.packet_lost, 0); - EXPECT_NEAR(time_estimate(0), 10.010, 0.001); - - time_synchronize(10.025, 3, 0); - EXPECT_EQ(g_odometry.packet_lost, 0); - EXPECT_NEAR(time_estimate(0), 10.015, 0.001); - EXPECT_NEAR(time_estimate(1), 10.020, 0.001); - EXPECT_NEAR(time_estimate(2), 10.025, 0.001); - - time_synchronize(10.030, 1, 0); - EXPECT_EQ(g_odometry.packet_lost, 0); - EXPECT_NEAR(time_estimate(0), 10.030, 0.001); - - time_synchronize(10.050, 1, 0); // lost 3 - EXPECT_EQ(g_odometry.packet_lost, 3); - EXPECT_NEAR(time_estimate(0), 10.050, 0.001); - - time_synchronize(10.055, 2, 0); // recover 1, lost 2 - EXPECT_EQ(g_odometry.packet_lost, 2); - EXPECT_NEAR(time_estimate(0), 10.050, 0.001); - EXPECT_NEAR(time_estimate(1), 10.055, 0.001); -} - -TEST(TimeSynchronize, Reset) -{ - g_param.output_lv = OUTPUT_LV_DEBUG; - g_param.option = OPTION_SHOW_TIMESTAMP; - g_interval = SER_INTERVAL; - g_offset = 0; - g_odometry.packet_lost = 0; - - time_synchronize(10.005, 1, 0); - EXPECT_EQ(g_odometry.packet_lost, 0); - EXPECT_NEAR(time_estimate(0), 10.005, 0.001); - - time_synchronize(10.010, 1, 0); - EXPECT_EQ(g_odometry.packet_lost, 0); - EXPECT_NEAR(time_estimate(0), 10.010, 0.001); - - time_synchronize(10.600, 1, 0); - EXPECT_EQ(g_odometry.packet_lost, 0); - EXPECT_NEAR(time_estimate(0), 10.600, 0.001); - - time_synchronize(10.605, 1, 0); - EXPECT_EQ(g_odometry.packet_lost, 0); - EXPECT_NEAR(time_estimate(0), 10.605, 0.001); -} - -TEST(TimeSynchronize, ClockGain) -{ - g_param.output_lv = OUTPUT_LV_DEBUG; - g_param.option = OPTION_SHOW_TIMESTAMP; - g_interval = SER_INTERVAL; - g_offset = 0; - g_odometry.packet_lost = 0; - - const double actual_intervals[] = { - 0.00495, - 0.00498, - 0.00502, - 0.00505, - }; - - for (const double actual_interval : actual_intervals) - { - int cnt = 0; - for (double t = 100; t < 160; t += actual_interval) - { - int readnum = 1; - if (++cnt % 10 == 0) - { - t += actual_interval; - readnum++; - } - - time_synchronize(t, readnum, 0); - ASSERT_EQ(g_odometry.packet_lost, 0); - for (int i = 0; i < readnum; i++) - { - ASSERT_NEAR(time_estimate(i), t - actual_interval * (readnum - 1 - i), 0.001) - << "interval: " << actual_interval << ", t: " << t << ", cnt: " << cnt << ", i: " << i; - } - } - - ASSERT_NEAR(actual_interval, g_interval, 0.00001) - << "interval: " << actual_interval; - } -} - -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - - return RUN_ALL_TESTS(); -}