diff --git a/.github/workflows/release_build.yml b/.github/workflows/release_build.yml index 505a1f1cc..c949cc6eb 100644 --- a/.github/workflows/release_build.yml +++ b/.github/workflows/release_build.yml @@ -41,14 +41,19 @@ jobs: name: Bazel Build runs-on: ${{ matrix.os }} strategy: + fail-fast: false matrix: include: # Do a singular build for Bazel 4.2.2 to test Bazel 4.2.2. + # + # Bazel 4.2.2 injects -std=c++11 by default; override to c++14 for + # googletest compatibility. - os: ubuntu-latest compiler: gcc version: 11 bazel: 4.2.2 - # Run Bazel 6.5.0 on all compilers versions. + cppstd: c++14 + # Run Bazel 6.5.0 on all compiler versions. - os: ubuntu-latest compiler: gcc version: 9 @@ -57,14 +62,36 @@ jobs: compiler: gcc version: 10 bazel: 6.5.0 + - os: ubuntu-latest + compiler: clang + version: 14 + bazel: 6.5.0 + # Test all C++ standards on Bazel 6.5.0 + GCC 11. - os: ubuntu-latest compiler: gcc version: 11 bazel: 6.5.0 + cppstd: c++11 - os: ubuntu-latest - compiler: clang - version: 14 + compiler: gcc + version: 11 bazel: 6.5.0 + cppstd: c++14 + - os: ubuntu-latest + compiler: gcc + version: 11 + bazel: 6.5.0 + cppstd: c++17 + - os: ubuntu-latest + compiler: gcc + version: 11 + bazel: 6.5.0 + cppstd: c++20 + - os: ubuntu-latest + compiler: gcc + version: 11 + bazel: 6.5.0 + cppstd: c++23 steps: - uses: actions/checkout@v2 @@ -114,14 +141,25 @@ jobs: echo "CC=/usr/bin/clang++-${{ matrix.version }}" >> $GITHUB_ENV fi + - name: Set C++ Standard + run: | + if [[ -n "${{ matrix.cppstd }}" ]]; then + echo "CXXOPT=--cxxopt=-std=${{ matrix.cppstd }}" >> $GITHUB_ENV + fi + - name: Build Library run: | - bazel build -c opt //:* + bazel build -c opt ${{ env.CXXOPT }} $(bazel query '//:all except tests(//:all)') - name: Build Examples run: | cd examples && - bazel build -c opt //:* + bazel build -c opt ${{ env.CXXOPT }} //:all + + - name: Run Unit Tests + if: matrix.cppstd != 'c++11' + run: | + bazel test -c opt ${{ env.CXXOPT }} //:all - name: Install Test Files run: | @@ -147,6 +185,7 @@ jobs: name: CMake Build runs-on: ${{ matrix.os }} strategy: + fail-fast: false matrix: include: - os: ubuntu-latest @@ -329,6 +368,7 @@ jobs: name: Python Unit Tests runs-on: ubuntu-latest strategy: + fail-fast: false matrix: python-version: ['3.8', '3.9', '3.10', '3.11', '3.12', '3.13'] defaults: diff --git a/BUILD b/BUILD index 7189a67c9..460df88e6 100644 --- a/BUILD +++ b/BUILD @@ -8,6 +8,7 @@ cc_library( ":messages", ":parsers", ":rtcm", + ":utils", ], ) @@ -56,6 +57,7 @@ cc_library( "src/point_one/fusion_engine/messages/measurements.h", "src/point_one/fusion_engine/messages/signal_defs.h", "src/point_one/fusion_engine/messages/solution.h", + "src/point_one/fusion_engine/messages/timestamp.h", ], deps = [ ":common", @@ -103,21 +105,6 @@ cc_library( includes = ["src"], ) -# Message encode/decode support. -cc_library( - name = "parsers", - srcs = [ - "src/point_one/fusion_engine/parsers/fusion_engine_framer.cc", - ], - hdrs = [ - "src/point_one/fusion_engine/parsers/fusion_engine_framer.h", - ], - deps = [ - ":core_headers", - ":crc", - ], -) - # CRC support. cc_library( name = "crc", @@ -148,6 +135,21 @@ cc_library( ) # Message encode/decode support. +cc_library( + name = "parsers", + srcs = [ + "src/point_one/fusion_engine/parsers/fusion_engine_framer.cc", + ], + hdrs = [ + "src/point_one/fusion_engine/parsers/fusion_engine_framer.h", + ], + deps = [ + ":core_headers", + ":crc", + ], +) + +# RTCM message framing support. cc_library( name = "rtcm", srcs = [ @@ -160,3 +162,43 @@ cc_library( ":common", ], ) + +# Helper utilities. +cc_library( + name = "utils", + srcs = [ + "src/point_one/fusion_engine/utils/time_provider.cc", + ], + hdrs = [ + "src/point_one/fusion_engine/utils/time_provider.h", + ], + deps = [ + ":common", + ":core_headers", + ], +) + +################################################################################ +# Tests +################################################################################ + +cc_test( + name = "timestamp_test", + size = "small", + srcs = ["src/point_one/fusion_engine/messages/timestamp_test.cc"], + deps = [ + ":core_headers", + "@googletest//:gtest_main", + ], +) + +cc_test( + name = "time_provider_test", + size = "small", + srcs = ["src/point_one/fusion_engine/utils/time_provider_test.cc"], + deps = [ + ":messages", + ":utils", + "@googletest//:gtest_main", + ], +) diff --git a/CMakeLists.txt b/CMakeLists.txt index b160db3e4..4638e0091 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -36,6 +36,7 @@ add_library(fusion_engine_client src/point_one/fusion_engine/messages/crc.cc src/point_one/fusion_engine/messages/data_version.cc src/point_one/fusion_engine/parsers/fusion_engine_framer.cc + src/point_one/fusion_engine/utils/time_provider.cc src/point_one/rtcm/rtcm_framer.cc) target_include_directories(fusion_engine_client PUBLIC ${PROJECT_SOURCE_DIR}/src) if (MSVC) diff --git a/README.md b/README.md index e6c968946..67df84942 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,6 @@ +
Build Status
C++ Support
Python Support
CodeQL
diff --git a/WORKSPACE b/WORKSPACE index b151a07b2..463fa6b8d 100644 --- a/WORKSPACE +++ b/WORKSPACE @@ -1 +1,12 @@ workspace(name = "p1_fusion_engine_client") + +load("@bazel_tools//tools/build_defs/repo:http.bzl", "http_archive") + +# Note: gtest requires C++14 or greater. The library is still compatible with +# C++11. +http_archive( + name = "googletest", + sha256 = "8ad598c73ad796e0d8280b082cebd82a630d73e73cd3c70057938a6501bba5d7", + strip_prefix = "googletest-1.14.0", + urls = ["https://github.com/google/googletest/archive/refs/tags/v1.14.0.tar.gz"], +) diff --git a/examples/.bazelversion b/examples/.bazelversion new file mode 100644 index 000000000..4be2c727a --- /dev/null +++ b/examples/.bazelversion @@ -0,0 +1 @@ +6.5.0 \ No newline at end of file diff --git a/examples/BUILD b/examples/BUILD index ce0020e21..d3a77e884 100644 --- a/examples/BUILD +++ b/examples/BUILD @@ -4,10 +4,13 @@ package(default_visibility = ["//visibility:public"]) filegroup( name = "examples", srcs = [ + "//convert_time", "//generate_data", + "//lband_decode", "//message_decode", + "//raw_message_decode", "//request_version", "//tcp_client", "//udp_client", - ] + ], ) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index c633a71d6..a9a5aa43d 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -1,5 +1,6 @@ add_subdirectory(common) +add_subdirectory(convert_time) add_subdirectory(generate_data) add_subdirectory(lband_decode) add_subdirectory(message_decode) diff --git a/examples/common/BUILD b/examples/common/BUILD index af007a128..b01d55e97 100644 --- a/examples/common/BUILD +++ b/examples/common/BUILD @@ -12,3 +12,16 @@ cc_library( "@fusion_engine_client", ], ) + +cc_library( + name = "time_provider", + srcs = [ + "time_provider.cc", + ], + hdrs = [ + "time_provider.h", + ], + deps = [ + "@fusion_engine_client", + ], +) diff --git a/examples/convert_time/BUILD b/examples/convert_time/BUILD new file mode 100644 index 000000000..5cc4e6ab2 --- /dev/null +++ b/examples/convert_time/BUILD @@ -0,0 +1,11 @@ +package(default_visibility = ["//visibility:public"]) + +cc_binary( + name = "convert_time", + srcs = [ + "convert_time.cc", + ], + deps = [ + "@fusion_engine_client", + ], +) diff --git a/examples/convert_time/CMakeLists.txt b/examples/convert_time/CMakeLists.txt new file mode 100644 index 000000000..6e534bd93 --- /dev/null +++ b/examples/convert_time/CMakeLists.txt @@ -0,0 +1,2 @@ +add_executable(convert_time convert_time.cc) +target_link_libraries(convert_time PUBLIC fusion_engine_client) diff --git a/examples/convert_time/convert_time.cc b/examples/convert_time/convert_time.cc new file mode 100644 index 000000000..81de48fa4 --- /dev/null +++ b/examples/convert_time/convert_time.cc @@ -0,0 +1,100 @@ +/**************************************************************************/ /** + * @brief Use the example `TimeProvider` class to convert between P1 and GPS + * time. + * @file + ******************************************************************************/ + +#include +#include + +#include +#include + +using namespace point_one::fusion_engine::messages; +using namespace point_one::fusion_engine::utils; + +/******************************************************************************/ +void PrintGPSTime(const Timestamp& gps_time) { + uint16_t week_number = 0; + double tow_sec = NAN; + if (gps_time.ToGPSWeekTOW(&week_number, &tow_sec)) { + printf("Week %u, TOW %.9f sec\n", week_number, tow_sec); + } else { + printf("\n"); + } +} + +/******************************************************************************/ +void PrintTimes(const Timestamp& p1_time, const Timestamp& gps_time) { + printf(" P1: %.9f sec\n", p1_time.ToSeconds()); + printf(" GPS: "); + PrintGPSTime(gps_time); +} + +/******************************************************************************/ +void PrintIMUMeasurement(const TimeProvider& time_provider, + const IMUOutput& imu_message) { + PrintTimes(imu_message.p1_time, time_provider.P1ToGPS(imu_message.p1_time)); +} + +/******************************************************************************/ +int main(int /*argc*/, const char** /*argv*/) { + // Define a time provider we'll use to convert between P1 and GPS time. + TimeProvider time_provider; + + // Step 1: Try to compute GPS time for an incoming IMU measurement. We have + // not gotten any pose messages yet, so we do not know the P1/GPS time + // relationship. + IMUOutput imu1; + imu1.p1_time = {0, 999000000}; + printf("IMU measurement 1:\n"); + PrintIMUMeasurement(time_provider, imu1); + + // Step 2: First pose message received. Update the time provider. After this, + // we can start converting P1 timestamps to GPS time. + MessageHeader pose_header; + pose_header.message_type = MessageType::POSE; + PoseMessage pose1; + pose1.p1_time = {1, 0}; + pose1.gps_time = Timestamp::FromGPSTime(2416, 288018.0); + printf("First pose message:\n"); + PrintTimes(pose1.p1_time, pose1.gps_time); + time_provider.HandleMessage(pose_header, &pose1); + + printf("IMU measurement 1 again (GPS time available):\n"); + PrintIMUMeasurement(time_provider, imu1); + + // Step 3: Convert the next IMU measurement's P1 time to GPS time. + IMUOutput imu2; + imu2.p1_time = {1, 100000500}; + printf("IMU measurement 2:\n"); + PrintIMUMeasurement(time_provider, imu2); + + // Step 4: Second pose message received. We now know the difference in rate + // between P1 and GPS time, and can interpolate to compute GPS time even more + // accurately. + // + // Here, we're simulating that the P1 clock is initially slightly faster than + // the GPS clock. + // + // Note that the rate of P1 time is steered to match GPS time over time. After + // GPS time has been available for a few seconds, the two rates will be very + // close and interpolation will only have a very minor effect. + PoseMessage pose2; + pose2.p1_time = {1, 100001000}; + pose2.gps_time = Timestamp::FromGPSTime(2416, 288018.1); + printf("Second pose message:\n"); + PrintTimes(pose2.p1_time, pose2.gps_time); + time_provider.HandleMessage(pose_header, &pose2); + + printf("IMU measurement 2 again (using interpolation):\n"); + PrintIMUMeasurement(time_provider, imu2); + + // Step 5: Convert a 3rd IMU measurement with interpolation applied. + IMUOutput imu3; + imu3.p1_time = {1, 100001500}; + printf("IMU measurement 3:\n"); + PrintIMUMeasurement(time_provider, imu3); + + return 0; +} diff --git a/python/fusion_engine_client/messages/timestamp.py b/python/fusion_engine_client/messages/timestamp.py index 931de507c..4ec1b2e1c 100644 --- a/python/fusion_engine_client/messages/timestamp.py +++ b/python/fusion_engine_client/messages/timestamp.py @@ -55,6 +55,9 @@ class Timestamp: def __init__(self, time_sec=math.nan): self.seconds = float(time_sec) + def is_valid(self) -> bool: + return not math.isnan(self.seconds) + def is_gps(self) -> bool: return is_gps_time(self.seconds) @@ -72,12 +75,26 @@ def as_utc(self) -> datetime: def get_week_tow(self) -> (int, float): if self.is_gps(): - week = int(self.seconds / SECONDS_PER_WEEK) - tow_sec = self.seconds - week * SECONDS_PER_WEEK - return week, tow_sec + week_number = int(self.seconds / SECONDS_PER_WEEK) + tow_sec = self.seconds - week_number * SECONDS_PER_WEEK + return week_number, tow_sec else: return -1, np.nan + @classmethod + def from_datetime(cls, time: Union[datetime, gpstime]) -> 'Timestamp': + if isinstance(time, gpstime): + return Timestamp(time.gps()) + else: + return Timestamp(gpstime.fromdatetime(time).gps()) + + @classmethod + def from_gps_week_tow(cls, week_number: int, tow_sec: float) -> 'Timestamp': + if week_number < 0 or tow_sec < 0.0 or not math.isfinite(tow_sec): + return Timestamp() + else: + return Timestamp(week_number * SECONDS_PER_WEEK + tow_sec) + def pack(self, buffer: bytes = None, offset: int = 0, return_buffer: bool = False) -> (bytes, int): if math.isnan(self.seconds): int_part = Timestamp._INVALID @@ -110,17 +127,37 @@ def calcsize(cls) -> int: return Timestamp._SIZE def __add__(self, other): - return Timestamp(self.seconds + float(other)) + if isinstance(other, timedelta): + return Timestamp(self.seconds + other.total_seconds()) + else: + return Timestamp(self.seconds + float(other)) + + def __radd__(self, other): + return self.__add__(other) def __sub__(self, other): - return Timestamp(self.seconds - float(other)) + if isinstance(other, Timestamp): + if self.is_valid() and other.is_valid(): + return timedelta(seconds=(self.seconds - other.seconds)) + else: + return None + elif isinstance(other, timedelta): + return Timestamp(self.seconds - other.total_seconds()) + else: + return Timestamp(self.seconds - float(other)) def __iadd__(self, other): - self.seconds += float(other) + if isinstance(other, timedelta): + self.seconds += other.total_seconds() + else: + self.seconds += float(other) return self - def __isub(self, other): - self.seconds -= float(other) + def __isub__(self, other): + if isinstance(other, timedelta): + self.seconds -= other.total_seconds() + else: + self.seconds -= float(other) return self def __eq__(self, other): @@ -142,7 +179,7 @@ def __ge__(self, other): return self.seconds >= float(other) def __bool__(self): - return not math.isnan(self.seconds) + return self.is_valid() def __float__(self): return self.seconds diff --git a/python/fusion_engine_client/utils/time_provider.py b/python/fusion_engine_client/utils/time_provider.py new file mode 100644 index 000000000..33b139ff4 --- /dev/null +++ b/python/fusion_engine_client/utils/time_provider.py @@ -0,0 +1,161 @@ +from typing import Optional, Union + +from datetime import datetime, timedelta + +from gpstime import gpstime + +from ..messages import MessageHeader, MessagePayload, PoseMessage, Timestamp +from ..utils import trace as logging + +_logger = logging.getLogger('point_one.fusion_engine.utils.time_provider') + + +class TimeProvider: + """! + @brief Utility for converting between P1 and GPS time. + """ + def __init__(self): + self._current_p1_time = Timestamp() + self._current_gps_time = Timestamp() + self._prev_p1_time = Timestamp() + self._prev_gps_time = Timestamp() + + def reset(self): + self._current_p1_time = Timestamp() + self._current_gps_time = Timestamp() + self._prev_p1_time = Timestamp() + self._prev_gps_time = Timestamp() + + def handle_message(self, message: MessagePayload, header: Optional[MessageHeader] = None): + """! + @brief Learn time relationships from incoming FusionEngine messages. + + @param header The message header (optional). + @param message The message payload. + """ + if isinstance(message, PoseMessage): + # Sanity check for duplicate or backwards timestamps. In practice this should not happen normally unless the + # device was reset. If time jumps backward, we'll assume it was a reset and store the new time. If we get a + # duplicate timestamp, we'll ignore it as a possible error. + if self._current_p1_time and message.p1_time: + dt_sec = (message.p1_time - self._current_p1_time).total_seconds() + if dt_sec < -1e-3: + _logger.warning(f'Backwards P1 time jump detected. Did the device restart? ' + f'[prev={self._current_p1_time.to_p1_str()}, ' + f'current={message.p1_time.to_p1_str()}, dt={dt_sec:.2f} sec]') + self.reset() + elif dt_sec < 1e-3: + _logger.warning(f'Duplicate P1 timestamp detected. Ignoring. ' + f'[prev={self._current_p1_time.to_p1_str()}, ' + f'current={message.p1_time.to_p1_str()}, dt={dt_sec:.2f} sec]') + return + + # Store the current and previous P1/GPS times, and use them to convert to/from P1 or GPS time by + # interpolating (or extrapolating as needed). + # + # Note: If we had GPS time and the incoming message no longer does, we will no longer be able to convert + # P1<->GPS time. + self._prev_p1_time = self._current_p1_time + self._prev_gps_time = self._current_gps_time + self._current_p1_time = message.p1_time + self._current_gps_time = message.gps_time + if _logger.isEnabledFor(logging.DEBUG): + if self._current_p1_time and self._current_gps_time and self._prev_p1_time and self._prev_gps_time: + scale_sps = ((self._current_p1_time - self._prev_p1_time).total_seconds() / + (self._current_gps_time - self._prev_gps_time).total_seconds()) + scale_sps_str = f'{scale_sps:.9f} sec/sec' + else: + scale_sps_str = '' + _logger.debug(f"""\ +Received time update ({message.get_type()} message) at: + P1: {self._current_p1_time.to_p1_str()} + GPS: {self._current_gps_time.to_gps_str()} + P1/GPS: {scale_sps_str} +""") + + def p1_to_gps(self, p1_time: Timestamp, format: str = 'timestamp') -> Union[Timestamp, datetime]: + """! + @brief Convert a P1 timestamp to GPS time. + + @param p1_time The P1 time to convert. + @param format The desired output format: + - `timestamp` - A FusionEngine @ref Timestamp object + - `datetime` - A Python `datetime` object with the corresponding UTC time + + @return The resulting GPS time, or an invalid timestamp if the time could not be converted. + """ + if not p1_time: + _logger.trace('Cannot convert invalid P1 time to GPS time.') + if format == 'datetime': + return None + else: + return Timestamp() + elif not self._current_p1_time or not self._current_gps_time: + if _logger.isEnabledFor(logging.TRACE): + _logger.trace(f'P1/GPS relationship not known. Cannot convert P1 {p1_time.to_p1_str()} to GPS time.') + if format == 'datetime': + return None + else: + return Timestamp() + + # If we have both P1 and GPS time from the previous update, interpolate (or extrapolate) between the previous + # update and the current one for the most accurate result. + if self._prev_p1_time and self._prev_gps_time: + elapsed_p1_sec = (self._current_p1_time - self._prev_p1_time).total_seconds() + elapsed_gps_sec = (self._current_gps_time - self._prev_gps_time).total_seconds() + delta_p1_sec = (p1_time - self._prev_p1_time).total_seconds() + delta_gps_sec = elapsed_gps_sec * delta_p1_sec / elapsed_p1_sec + gps_time = self._prev_gps_time + timedelta(seconds=delta_gps_sec) + # Otherwise, use the current P1/GPS time offset with no interpolation. This will be less accurate since it + # cannot account for drift between P1 and GPS time, but for most purposes it will be fine as long as + # _current_*_time is recent. + else: + offset_sec = (self._current_gps_time - self._current_p1_time).total_seconds() + gps_time = p1_time + offset_sec + + if _logger.isEnabledFor(logging.TRACE): + _logger.trace('Converted P1 %s to GPS %s.', p1_time.to_p1_str(), gps_time.to_gps_str()) + + if format == 'datetime': + return gpstime.fromgps(float(gps_time)) + else: + return gps_time + + def gps_to_p1(self, gps_time: Union[Timestamp, datetime, gpstime]) -> Timestamp: + """! + @brief Convert a GPS timestamp to P1 time. + + @param gps_time The GPS time (or UTC `datetime`) to convert. + + @return The resulting P1 time, or an invalid timestamp if the time could not be converted. + """ + if not gps_time: + _logger.trace('Cannot convert invalid GPS time to P1 time.') + return Timestamp() + elif isinstance(gps_time, (datetime, gpstime)): + gps_time = Timestamp.from_datetime(gps_time) + + if not self._current_gps_time or not self._current_p1_time: + if _logger.isEnabledFor(logging.TRACE): + _logger.trace(f'GPS/P1 relationship not known. Cannot convert GPS {gps_time.to_gps_str()} to P1 time.') + return Timestamp() + + # If we have both GPS and P1 time from the previous update, interpolate (or extrapolate) between the previous + # update and the current one for the most accurate result. + if self._prev_gps_time and self._prev_p1_time: + elapsed_p1_sec = (self._current_p1_time - self._prev_p1_time).total_seconds() + elapsed_gps_sec = (self._current_gps_time - self._prev_gps_time).total_seconds() + delta_gps_sec = (gps_time - self._prev_gps_time).total_seconds() + delta_p1_sec = elapsed_p1_sec * delta_gps_sec / elapsed_gps_sec + p1_time = self._prev_p1_time + timedelta(seconds=delta_p1_sec) + # Otherwise, use the current GPS/P1 time offset with no interpolation. This will be less accurate since it + # cannot account for drift between GPS and P1 time, but for most purposes it will be fine as long as + # _current_*_time is recent. + else: + offset_sec = (self._current_p1_time - self._current_gps_time).total_seconds() + p1_time = gps_time + offset_sec + + if _logger.isEnabledFor(logging.TRACE): + _logger.trace('Converted GPS %s to P1 %s.', gps_time.to_gps_str(), p1_time.to_p1_str()) + return p1_time + diff --git a/python/tests/test_time_provider.py b/python/tests/test_time_provider.py new file mode 100644 index 000000000..a60372f3a --- /dev/null +++ b/python/tests/test_time_provider.py @@ -0,0 +1,236 @@ +from datetime import datetime + +from gpstime import gpstime +import pytest + +from fusion_engine_client.messages import PoseMessage, Timestamp +from fusion_engine_client.utils.time_provider import TimeProvider + + +# GPS time for 2026/4/29 08:00:00 UTC (arbitrary reference point for the tests below). +GPS_DATE_SEC = 1461484818.0 + + +def _make_pose(p1_sec, gps_sec): + msg = PoseMessage() + msg.p1_time = Timestamp(p1_sec) + msg.gps_time = Timestamp(gps_sec) + return msg + + +class TestHandleMessage: + def test_ignores_non_pose_message(self): + tp = TimeProvider() + # Passing a non-PoseMessage should not crash and leave state invalid. + tp.handle_message(object()) + assert not tp._current_p1_time + assert not tp._current_gps_time + + def test_stores_current_times(self): + tp = TimeProvider() + tp.handle_message(_make_pose(10.0, GPS_DATE_SEC)) + assert float(tp._current_p1_time) == pytest.approx(10.0) + assert float(tp._current_gps_time) == pytest.approx(GPS_DATE_SEC) + + def test_advances_prev_times(self): + tp = TimeProvider() + tp.handle_message(_make_pose(10.0, GPS_DATE_SEC)) + tp.handle_message(_make_pose(11.0, GPS_DATE_SEC + 1.0)) + assert float(tp._prev_p1_time) == pytest.approx(10.0) + assert float(tp._prev_gps_time) == pytest.approx(GPS_DATE_SEC) + assert float(tp._current_p1_time) == pytest.approx(11.0) + assert float(tp._current_gps_time) == pytest.approx(GPS_DATE_SEC + 1.0) + + # --- Backwards timestamp tests --- + + def test_backwards_timestamp_resets_and_stores_new(self): + # A large backwards jump resets state and stores the new (earlier) time. + tp = TimeProvider() + tp.handle_message(_make_pose(10.0, GPS_DATE_SEC)) + tp.handle_message(_make_pose(5.0, GPS_DATE_SEC - 5.0)) + assert float(tp._current_p1_time) == pytest.approx(5.0) + assert float(tp._current_gps_time) == pytest.approx(GPS_DATE_SEC - 5.0) + assert not tp._prev_p1_time + assert not tp._prev_gps_time + + def test_backwards_timestamp_after_two_updates_resets_prev(self): + # After two good updates, a backwards jump clears prev so conversion falls back + # to single-reference mode with only the new (reset) time. + tp = TimeProvider() + tp.handle_message(_make_pose(10.0, GPS_DATE_SEC)) + tp.handle_message(_make_pose(20.0, GPS_DATE_SEC + 10.0)) + tp.handle_message(_make_pose(5.0, GPS_DATE_SEC - 5.0)) + assert float(tp._current_p1_time) == pytest.approx(5.0) + assert not tp._prev_p1_time + + def test_backwards_conversion_uses_new_reference(self): + # After a backwards reset the new single reference is used for conversion. + tp = TimeProvider() + tp.handle_message(_make_pose(10.0, GPS_DATE_SEC)) + tp.handle_message(_make_pose(5.0, GPS_DATE_SEC - 5.0)) + result = tp.p1_to_gps(Timestamp(7.0)) + assert float(result) == pytest.approx(GPS_DATE_SEC - 3.0) + + def test_tiny_backwards_jump_within_threshold_treated_as_duplicate(self): + # A backwards jump of exactly 0.5 ms (< 1 ms) is treated as a duplicate and ignored. + tp = TimeProvider() + tp.handle_message(_make_pose(10.0, GPS_DATE_SEC)) + tp.handle_message(_make_pose(10.0 - 0.0005, GPS_DATE_SEC - 0.0005)) + assert float(tp._current_p1_time) == pytest.approx(10.0) + assert not tp._prev_p1_time + + # --- Duplicate timestamp tests --- + + def test_exact_duplicate_is_ignored(self): + # Sending the same p1_time twice: second message should be dropped. + tp = TimeProvider() + tp.handle_message(_make_pose(10.0, GPS_DATE_SEC)) + tp.handle_message(_make_pose(10.0, GPS_DATE_SEC)) + assert float(tp._current_p1_time) == pytest.approx(10.0) + assert not tp._prev_p1_time + + def test_near_duplicate_below_threshold_is_ignored(self): + # A 0.5 ms forward jump (below the 1 ms threshold) is treated as a duplicate. + tp = TimeProvider() + tp.handle_message(_make_pose(10.0, GPS_DATE_SEC)) + tp.handle_message(_make_pose(10.0005, GPS_DATE_SEC + 0.0005)) + assert float(tp._current_p1_time) == pytest.approx(10.0) + assert not tp._prev_p1_time + + def test_near_duplicate_above_threshold_is_accepted(self): + # A 2 ms forward jump (above the 1 ms threshold) is treated as a new update. + tp = TimeProvider() + tp.handle_message(_make_pose(10.0, GPS_DATE_SEC)) + tp.handle_message(_make_pose(10.002, GPS_DATE_SEC + 0.002)) + assert float(tp._current_p1_time) == pytest.approx(10.002) + assert float(tp._prev_p1_time) == pytest.approx(10.0) + + def test_duplicate_does_not_corrupt_conversion(self): + # After a duplicate is dropped, conversion still works correctly using the + # original reference. + tp = TimeProvider() + tp.handle_message(_make_pose(10.0, GPS_DATE_SEC)) + tp.handle_message(_make_pose(10.0, GPS_DATE_SEC)) # duplicate + result = tp.p1_to_gps(Timestamp(12.0)) + assert float(result) == pytest.approx(GPS_DATE_SEC + 2.0) + + +class TestP1ToGPS: + def test_invalid_p1_returns_invalid(self): + tp = TimeProvider() + result = tp.p1_to_gps(Timestamp()) + assert not result + + def test_no_reference_returns_invalid(self): + tp = TimeProvider() + result = tp.p1_to_gps(Timestamp(10.0)) + assert not result + + def test_invalid_p1_returns_none_for_datetime_format(self): + tp = TimeProvider() + assert tp.p1_to_gps(Timestamp(), format='datetime') is None + + def test_no_reference_returns_none_for_datetime_format(self): + tp = TimeProvider() + assert tp.p1_to_gps(Timestamp(10.0), format='datetime') is None + + def test_single_reference_no_interpolation(self): + tp = TimeProvider() + tp.handle_message(_make_pose(10.0, GPS_DATE_SEC)) + # Offset is GPS_2021_SEC - 10.0; querying p1=12.0 should yield GPS_2021_SEC + 2.0. + result = tp.p1_to_gps(Timestamp(12.0)) + assert float(result) == pytest.approx(GPS_DATE_SEC + 2.0) + + def test_two_references_interpolation(self): + tp = TimeProvider() + tp.handle_message(_make_pose(10.0, GPS_DATE_SEC)) + tp.handle_message(_make_pose(20.0, GPS_DATE_SEC + 10.0)) + # Midpoint between the two updates. + result = tp.p1_to_gps(Timestamp(15.0)) + assert float(result) == pytest.approx(GPS_DATE_SEC + 5.0) + + def test_two_references_extrapolation(self): + tp = TimeProvider() + tp.handle_message(_make_pose(10.0, GPS_DATE_SEC)) + tp.handle_message(_make_pose(20.0, GPS_DATE_SEC + 10.0)) + # Past the latest update — extrapolates. + result = tp.p1_to_gps(Timestamp(25.0)) + assert float(result) == pytest.approx(GPS_DATE_SEC + 15.0) + + def test_interpolation_with_drift(self): + # P1 runs slightly fast: 10 P1-sec == 10.001 GPS-sec. + tp = TimeProvider() + tp.handle_message(_make_pose(10.0, GPS_DATE_SEC)) + tp.handle_message(_make_pose(20.0, GPS_DATE_SEC + 10.001)) + result = tp.p1_to_gps(Timestamp(15.0)) + assert float(result) == pytest.approx(GPS_DATE_SEC + 5.0005) + + def test_datetime_format(self): + tp = TimeProvider() + tp.handle_message(_make_pose(10.0, GPS_DATE_SEC)) + result = tp.p1_to_gps(Timestamp(10.0), format='datetime') + assert isinstance(result, datetime) + + def test_datetime_format_matches_timestamp_format(self): + tp = TimeProvider() + tp.handle_message(_make_pose(10.0, GPS_DATE_SEC)) + ts_result = tp.p1_to_gps(Timestamp(12.0)) + dt_result = tp.p1_to_gps(Timestamp(12.0), format='datetime') + assert isinstance(dt_result, datetime) + expected = gpstime.fromgps(float(ts_result)) + assert dt_result == expected + + +class TestGPSToP1: + def test_invalid_gps_returns_invalid(self): + tp = TimeProvider() + result = tp.gps_to_p1(Timestamp()) + assert not result + + def test_no_reference_returns_invalid(self): + tp = TimeProvider() + result = tp.gps_to_p1(Timestamp(GPS_DATE_SEC)) + assert not result + + def test_single_reference_no_interpolation(self): + tp = TimeProvider() + tp.handle_message(_make_pose(10.0, GPS_DATE_SEC)) + result = tp.gps_to_p1(Timestamp(GPS_DATE_SEC + 2.0)) + assert float(result) == pytest.approx(12.0) + + def test_two_references_interpolation(self): + tp = TimeProvider() + tp.handle_message(_make_pose(10.0, GPS_DATE_SEC)) + tp.handle_message(_make_pose(20.0, GPS_DATE_SEC + 10.0)) + result = tp.gps_to_p1(Timestamp(GPS_DATE_SEC + 5.0)) + assert float(result) == pytest.approx(15.0) + + def test_two_references_extrapolation(self): + tp = TimeProvider() + tp.handle_message(_make_pose(10.0, GPS_DATE_SEC)) + tp.handle_message(_make_pose(20.0, GPS_DATE_SEC + 10.0)) + result = tp.gps_to_p1(Timestamp(GPS_DATE_SEC + 15.0)) + assert float(result) == pytest.approx(25.0) + + def test_accepts_datetime(self): + tp = TimeProvider() + tp.handle_message(_make_pose(10.0, GPS_DATE_SEC)) + dt = gpstime.fromgps(GPS_DATE_SEC + 2.0) + result = tp.gps_to_p1(dt) + assert float(result) == pytest.approx(12.0, abs=1e-3) + + def test_accepts_gpstime(self): + tp = TimeProvider() + tp.handle_message(_make_pose(10.0, GPS_DATE_SEC)) + gt = gpstime.fromgps(GPS_DATE_SEC + 2.0) + result = tp.gps_to_p1(gt) + assert float(result) == pytest.approx(12.0, abs=1e-3) + + def test_roundtrip_p1_to_gps_to_p1(self): + tp = TimeProvider() + tp.handle_message(_make_pose(10.0, GPS_DATE_SEC)) + tp.handle_message(_make_pose(20.0, GPS_DATE_SEC + 10.0)) + original = Timestamp(14.5) + gps = tp.p1_to_gps(original) + recovered = tp.gps_to_p1(gps) + assert float(recovered) == pytest.approx(float(original), abs=1e-6) diff --git a/python/tests/test_timestamp.py b/python/tests/test_timestamp.py new file mode 100644 index 000000000..dfb12069d --- /dev/null +++ b/python/tests/test_timestamp.py @@ -0,0 +1,322 @@ +from datetime import datetime, timedelta +import math +import struct + +from gpstime import gpstime +import pytest + +from fusion_engine_client.messages.timestamp import ( + GPS_POSIX_EPOCH, + SECONDS_PER_WEEK, + Timestamp, + Y2K_GPS_SEC, + is_gps_time, +) + +# GPS time for 2026/4/29 08:00:00 UTC — same reference used in test_time_provider.py. +GPS_DATE_SEC = 1461484818.0 + +GPS_DATE_WEEK = int(GPS_DATE_SEC / SECONDS_PER_WEEK) +GPS_DATE_TOW = GPS_DATE_SEC - GPS_DATE_WEEK * SECONDS_PER_WEEK + + +class TestConstruction: + def test_default_is_invalid(self): + assert not Timestamp() + + def test_nan_is_invalid(self): + assert not Timestamp(math.nan) + + def test_zero_is_valid(self): + assert Timestamp(0.0) + + def test_positive_is_valid(self): + assert Timestamp(10.0) + + def test_stores_seconds(self): + assert float(Timestamp(10.5)) == pytest.approx(10.5) + + def test_fractional_seconds_preserved(self): + assert float(Timestamp(123.456789)) == pytest.approx(123.456789) + + +class TestIsValid: + def test_default_invalid(self): + assert not Timestamp().is_valid() + + def test_valid_timestamp(self): + assert Timestamp(10.0).is_valid() + + def test_bool_false_for_invalid(self): + assert bool(Timestamp()) is False + + def test_bool_true_for_valid(self): + assert bool(Timestamp(10.0)) is True + + +class TestIsGPS: + def test_small_p1_time_not_gps(self): + assert not Timestamp(10.0).is_gps() + + def test_gps_time_is_gps(self): + assert Timestamp(GPS_DATE_SEC).is_gps() + + def test_y2k_boundary_is_gps(self): + assert Timestamp(Y2K_GPS_SEC).is_gps() + + def test_just_below_y2k_not_gps(self): + assert not Timestamp(Y2K_GPS_SEC - 1).is_gps() + + def test_is_gps_time_helper_scalar(self): + assert is_gps_time(GPS_DATE_SEC) + assert not is_gps_time(10.0) + + def test_as_gps_returns_datetime_for_gps_time(self): + result = Timestamp(GPS_DATE_SEC).as_gps() + expected = GPS_POSIX_EPOCH + timedelta(seconds=GPS_DATE_SEC) + assert result == expected + + def test_as_gps_returns_none_for_p1_time(self): + assert Timestamp(10.0).as_gps() is None + + def test_as_utc_returns_datetime_for_gps_time(self): + result = Timestamp(GPS_DATE_SEC).as_utc() + assert isinstance(result, datetime) + assert result.tzinfo is not None + + def test_as_utc_returns_none_for_p1_time(self): + assert Timestamp(10.0).as_utc() is None + + def test_as_gps_and_as_utc_differ_by_leap_seconds(self): + gps_dt = Timestamp(GPS_DATE_SEC).as_gps() + utc_dt = Timestamp(GPS_DATE_SEC).as_utc() + delta = abs((gps_dt - utc_dt).total_seconds()) + # GPS is ahead of UTC by 18 leap seconds of 2017. + assert delta == 18 + + +class TestGetWeekTow: + def test_gps_time_returns_correct_week_tow(self): + week, tow = Timestamp(GPS_DATE_SEC).get_week_tow() + assert week == GPS_DATE_WEEK + assert tow == pytest.approx(GPS_DATE_TOW) + + def test_week_tow_reconstructs_original(self): + week, tow = Timestamp(GPS_DATE_SEC).get_week_tow() + assert week * SECONDS_PER_WEEK + tow == pytest.approx(GPS_DATE_SEC) + + def test_p1_time_returns_invalid(self): + week, tow = Timestamp(10.0).get_week_tow() + assert week == -1 + assert math.isnan(tow) + + +class TestFromDatetime: + def test_from_gpstime_roundtrip(self): + gt = gpstime.fromgps(GPS_DATE_SEC) + ts = Timestamp.from_datetime(gt) + assert float(ts) == pytest.approx(GPS_DATE_SEC, abs=1e-3) + + def test_from_utc_datetime_roundtrip(self): + dt = gpstime.fromgps(GPS_DATE_SEC) + ts = Timestamp.from_datetime(dt) + assert float(ts) == pytest.approx(GPS_DATE_SEC, abs=1e-3) + + def test_from_datetime_is_gps(self): + dt = gpstime.fromgps(GPS_DATE_SEC) + ts = Timestamp.from_datetime(dt) + assert ts.is_gps() + + +class TestFromGPSWeekTow: + def test_basic_conversion(self): + result = Timestamp.from_gps_week_tow(GPS_DATE_WEEK, GPS_DATE_TOW) + assert float(result) == pytest.approx(GPS_DATE_SEC) + + def test_roundtrip_with_get_week_tow(self): + week, tow = Timestamp(GPS_DATE_SEC).get_week_tow() + result = Timestamp.from_gps_week_tow(week, tow) + assert float(result) == pytest.approx(GPS_DATE_SEC) + + def test_week_zero_tow_zero(self): + result = Timestamp.from_gps_week_tow(0, 0.0) + assert result.is_valid() + assert float(result) == pytest.approx(0.0) + + def test_fractional_tow_preserved(self): + result = Timestamp.from_gps_week_tow(GPS_DATE_WEEK, GPS_DATE_TOW + 0.123456) + assert float(result) == pytest.approx(GPS_DATE_SEC + 0.123456) + + def test_result_is_gps(self): + result = Timestamp.from_gps_week_tow(GPS_DATE_WEEK, GPS_DATE_TOW) + assert result.is_gps() + + def test_negative_week_returns_invalid(self): + assert not Timestamp.from_gps_week_tow(-1, 0.0) + + def test_negative_tow_returns_invalid(self): + assert not Timestamp.from_gps_week_tow(GPS_DATE_WEEK, -1.0) + + def test_nan_tow_returns_invalid(self): + assert not Timestamp.from_gps_week_tow(GPS_DATE_WEEK, math.nan) + + def test_infinite_tow_returns_invalid(self): + assert not Timestamp.from_gps_week_tow(GPS_DATE_WEEK, math.inf) + + +class TestArithmetic: + def test_add_float(self): + result = Timestamp(10.0) + 5.0 + assert isinstance(result, Timestamp) + assert float(result) == pytest.approx(15.0) + + def test_radd_float(self): + result = 5.0 + Timestamp(10.0) + assert isinstance(result, Timestamp) + assert float(result) == pytest.approx(15.0) + + def test_add_timedelta(self): + result = Timestamp(10.0) + timedelta(seconds=3) + assert isinstance(result, Timestamp) + assert float(result) == pytest.approx(13.0) + + def test_sub_float(self): + result = Timestamp(10.0) - 3.0 + assert isinstance(result, Timestamp) + assert float(result) == pytest.approx(7.0) + + def test_sub_timedelta(self): + result = Timestamp(10.0) - timedelta(seconds=3) + assert isinstance(result, Timestamp) + assert float(result) == pytest.approx(7.0) + + def test_sub_timestamp_returns_timedelta(self): + result = Timestamp(10.0) - Timestamp(3.0) + assert isinstance(result, timedelta) + assert result.total_seconds() == pytest.approx(7.0) + + def test_sub_invalid_timestamp_returns_none(self): + assert (Timestamp(10.0) - Timestamp()) is None + assert (Timestamp() - Timestamp(10.0)) is None + + def test_iadd_float(self): + ts = Timestamp(10.0) + ts += 5.0 + assert float(ts) == pytest.approx(15.0) + + def test_iadd_timedelta(self): + ts = Timestamp(10.0) + ts += timedelta(seconds=5) + assert float(ts) == pytest.approx(15.0) + + def test_isub_float(self): + ts = Timestamp(10.0) + ts -= 3.0 + assert float(ts) == pytest.approx(7.0) + + def test_isub_timedelta(self): + ts = Timestamp(10.0) + ts -= timedelta(seconds=3) + assert float(ts) == pytest.approx(7.0) + + +class TestComparison: + def test_eq(self): + assert Timestamp(10.0) == Timestamp(10.0) + assert not (Timestamp(10.0) == Timestamp(11.0)) + + def test_ne(self): + assert Timestamp(10.0) != Timestamp(11.0) + assert not (Timestamp(10.0) != Timestamp(10.0)) + + def test_lt(self): + assert Timestamp(10.0) < Timestamp(11.0) + assert not (Timestamp(11.0) < Timestamp(10.0)) + + def test_le(self): + assert Timestamp(10.0) <= Timestamp(10.0) + assert Timestamp(10.0) <= Timestamp(11.0) + assert not (Timestamp(11.0) <= Timestamp(10.0)) + + def test_gt(self): + assert Timestamp(11.0) > Timestamp(10.0) + assert not (Timestamp(10.0) > Timestamp(11.0)) + + def test_ge(self): + assert Timestamp(10.0) >= Timestamp(10.0) + assert Timestamp(11.0) >= Timestamp(10.0) + assert not (Timestamp(10.0) >= Timestamp(11.0)) + + def test_compare_with_raw_float(self): + assert Timestamp(10.0) == 10.0 + assert Timestamp(10.0) < 11.0 + assert Timestamp(10.0) > 9.0 + + +class TestPackUnpack: + def test_calcsize(self): + assert Timestamp.calcsize() == 8 + + def test_pack_returns_size_by_default(self): + result = Timestamp(10.0).pack() + assert result == 8 + + def test_pack_return_buffer(self): + buf = Timestamp(10.0).pack(return_buffer=True) + assert isinstance(buf, bytes) + assert len(buf) == 8 + + def test_pack_unpack_roundtrip(self): + original = Timestamp(10.5) + buf = original.pack(return_buffer=True) + recovered = Timestamp() + recovered.unpack(buf) + assert float(recovered) == pytest.approx(10.5, abs=1e-9) + + def test_pack_unpack_fractional(self): + original = Timestamp(GPS_DATE_SEC + 0.123456789) + buf = original.pack(return_buffer=True) + recovered = Timestamp() + recovered.unpack(buf) + assert float(recovered) == pytest.approx(float(original), abs=1e-6) + + def test_pack_invalid_uses_sentinel(self): + buf = Timestamp().pack(return_buffer=True) + int_part, frac_part = struct.unpack_from(' // For NAN +#include + +#include "point_one/fusion_engine/common/portability.h" + +namespace point_one { +namespace fusion_engine { +namespace messages { + +/** + * @brief Generic timestamp representation. + * @ingroup messages + * + * This structure may be used to store Point One system time values (referenced + * to the start of the device), UNIX times (referenced to January 1, 1970), or + * GPS times (referenced to January 6, 1980). + */ +struct P1_ALIGNAS(4) Timestamp { + static constexpr uint32_t INVALID = 0xFFFFFFFF; + + /** + * The number of full seconds since the epoch. Set to @ref INVALID if + * the timestamp is invalid or unknown. + */ + uint32_t seconds = INVALID; + + /** The fractional part of the second, expressed in nanoseconds. */ + uint32_t fraction_ns = INVALID; + + /** @brief Construct an invalid timestamp. */ + Timestamp() = default; + + /** + * @brief Construct a timestamp. + * + * @param sec The whole-seconds component. + * @param ns The nanoseconds component. + */ + Timestamp(uint32_t sec, uint32_t ns) : seconds(sec), fraction_ns(ns) {} + + /** + * @brief Check if this is a valid timestamp. + * + * @return `true` if the timestamp is valid. + */ + bool IsValid() const { return seconds != INVALID && fraction_ns != INVALID; } + + /** + * @brief Get the timestamp value in seconds. + * + * @return The timestamp value (in seconds), or `NAN` if invalid. + */ + double ToSeconds() const { + if (IsValid()) { + return seconds + (fraction_ns * 1e-9); + } else { + return NAN; + } + } + + /** + * @brief Convert a timestamp to GPS week number and time of week. + * + * @param week_number Set to the GPS week number. + * @param tow_sec Set to the GPS time of week (in seconds). + * + * @return `true` on success, `false` if the timestamp is invalid. + */ + bool ToGPSWeekTOW(uint16_t* week_number, double* tow_sec) const { + if (IsValid()) { + if (week_number) { + *week_number = static_cast(seconds / 604800); + } + if (tow_sec) { + *tow_sec = (seconds % 604800) + (fraction_ns * 1e-9); + } + return true; + } else { + return false; + } + } + + /** + * @brief Check if this is valid timestamp. + * + * @return `true` if the timestamp is valid. + */ + operator bool() const { return IsValid(); } + + /** + * @brief Construct a @ref Timestamp object from a GPS week number and time of + * week. + * + * @param week_number The GPS week number. + * @param tow_sec The GPS time of week (in seconds). + * + * @return The resulting @ref Timestamp. + */ + static Timestamp FromGPSTime(uint16_t week_number, double tow_sec) { + Timestamp result; + if (std::isfinite(tow_sec) && tow_sec >= 0.0) { + uint32_t tow_sec_int = static_cast(tow_sec); + result.seconds = week_number * 604800 + tow_sec_int; + result.fraction_ns = + static_cast(std::lround((tow_sec - tow_sec_int) * 1e9)); + if (result.fraction_ns >= 1000000000) { + ++result.seconds; + result.fraction_ns -= 1000000000; + } + } + return result; + } +}; + +/** + * @brief Represents a signed duration or time difference between two @ref + * Timestamp values. + * + * Unlike @ref Timestamp, which represents an absolute point in time using + * unsigned fields, `TimeDelta` uses signed integers to express durations that + * may be negative (i.e., in the past relative to a reference time). + * + * Both @ref seconds and @ref fraction_ns always share the same sign: both + * fields will be negative to represent negative values. For example, -1.5 + * seconds is represented as `{seconds=-1, fraction_ns=-500000000}`, never as + * `{seconds=0, fraction_ns=-1500000000}` or `{seconds=-2, + * fraction_ns=500000000}`. + * + * Both fields are set to @ref INVALID (`INT32_MIN`) to indicate an invalid or + * unknown delta. + */ +struct TimeDelta { + static constexpr int32_t INVALID = INT32_MIN; + + /** + * The number of full seconds in the delta. Negative for deltas in the past. + * Set to @ref INVALID if the delta is invalid or unknown. + */ + int32_t seconds = INVALID; + + /** + * The fractional part of the second, expressed in nanoseconds. + * + * Always has the same sign as @ref seconds (or is zero). Valid range is + * `[-999,999,999, 999,999,999]`. Set to @ref INVALID if the delta is + * invalid or unknown. + */ + int32_t fraction_ns = INVALID; + + /** @brief Construct an invalid delta. */ + TimeDelta() = default; + + /** + * @brief Construct a delta from seconds and nanoseconds, normalizing as + * needed. + * + * `fraction_ns` may exceed ±999,999,999; any excess is folded into + * @ref seconds. For example, `TimeDelta(0, 1500000000)` produces + * `{seconds=1, fraction_ns=500000000}`. + * + * @param sec The whole-seconds component. + * @param ns The nanoseconds component. May be larger than 1 sec. + */ + TimeDelta(int32_t sec, int32_t ns) : seconds(sec), fraction_ns(ns) { + if (seconds != INVALID && fraction_ns != INVALID) { + Normalize(); + } + } + + /** + * @brief Construct a delta from a floating point second value. + * + * @param sec The second value. + */ + TimeDelta(double sec) { + if (std::isfinite(sec) && sec <= INT32_MAX && + sec >= static_cast(INT32_MIN) - 1.0) { + seconds = static_cast(sec); + fraction_ns = static_cast(std::lround((sec - seconds) * 1e9)); + Normalize(); + } + } + + /** + * @brief Check if this delta is valid. + * + * @return `true` if the delta is valid. + */ + bool IsValid() const { return seconds != INVALID && fraction_ns != INVALID; } + + /** + * @brief Convert this delta to a floating-point number of seconds. + * + * @return The delta expressed as seconds, or `NAN` if the delta is + * invalid. + */ + double ToSeconds() const { + if (IsValid()) { + return seconds + (fraction_ns * 1e-9); + } else { + return NAN; + } + } + + /** + * @brief Normalize the delta so that @ref seconds and @ref fraction_ns + * share the same sign. + * + * After any arithmetic operation, this ensures the invariant that + * `fraction_ns` is always in the range `(-999,999,999, 999,999,999)` and + * has the same sign as `seconds` (or is zero). For example, + * `{seconds=1, fraction_ns=-200000000}` is normalized to + * `{seconds=0, fraction_ns=800000000}`. + * + * This is called automatically by the two-argument constructor and all + * arithmetic operators and does not need to be called manually under normal + * use. + */ + void Normalize() { + // Convert |fraction_ns| to <1 second. This must be done before sign + // alignment. + if (fraction_ns >= 1000000000) { + seconds += fraction_ns / 1000000000; + fraction_ns %= 1000000000; + } else if (fraction_ns <= -1000000000) { + seconds += fraction_ns / 1000000000; + fraction_ns %= 1000000000; + } + + // Align signs for second and fraction_ns. We already guaranteed fraction_ns + // is <1 sec, so no % operation needed. + if (fraction_ns > 0 && seconds < 0) { + seconds += 1; + fraction_ns -= 1000000000; + } else if (fraction_ns < 0 && seconds > 0) { + seconds -= 1; + fraction_ns += 1000000000; + } + } + + /** + * @brief Check if this is a valid delta. + * + * @return `true` if the delta is valid. + */ + operator bool() const { return IsValid(); } + + /** + * @brief Add another @ref TimeDelta to this one in place. + * + * If either operand is invalid, the result is set to @ref INVALID. + * + * @param rhs The delta to add. + * + * @return A reference to this delta after addition. + */ + TimeDelta& operator+=(const TimeDelta& rhs) { + if (IsValid()) { + if (rhs.IsValid()) { + seconds += rhs.seconds; + fraction_ns += rhs.fraction_ns; + Normalize(); + } else { + *this = {INVALID, INVALID}; + } + } + return *this; + } + + /** + * @brief Subtract another @ref TimeDelta from this one in place. + * + * If either operand is invalid, the result is set to @ref INVALID. + * + * @param rhs The delta to subtract. + * + * @return A reference to this delta after subtraction. + */ + TimeDelta& operator-=(const TimeDelta& rhs) { + if (IsValid()) { + if (rhs.IsValid()) { + seconds -= rhs.seconds; + fraction_ns -= rhs.fraction_ns; + Normalize(); + } else { + *this = {INVALID, INVALID}; + } + } + return *this; + } + + /** + * @brief Add two @ref TimeDelta values. + * + * If either operand is invalid, the result is @ref INVALID. + * + * @param lhs The left-hand operand. + * @param rhs The right-hand operand. + * + * @return The sum of the two deltas. + */ + friend TimeDelta operator+(TimeDelta lhs, const TimeDelta& rhs) { + lhs += rhs; + return lhs; + } + + /** + * @brief Subtract one @ref TimeDelta from another. + * + * If either operand is invalid, the result is @ref INVALID. + * + * @param lhs The left-hand operand. + * @param rhs The delta to subtract. + * + * @return The difference of the two deltas. + */ + friend TimeDelta operator-(TimeDelta lhs, const TimeDelta& rhs) { + lhs -= rhs; + return lhs; + } +}; + +/** + * @brief Compute the difference between two @ref Timestamp values as a + * @ref TimeDelta. + * + * @param lhs The timestamp to subtract from. + * @param rhs The timestamp to subtract. + * + * @return A @ref TimeDelta representing `lhs - rhs`, or an invalid + * delta if either operand is invalid. + */ +inline TimeDelta operator-(const Timestamp& lhs, const Timestamp& rhs) { + if (!lhs.IsValid() || !rhs.IsValid()) { + return TimeDelta(); + } + + TimeDelta result; + result.seconds = + static_cast(lhs.seconds) - static_cast(rhs.seconds); + result.fraction_ns = static_cast(lhs.fraction_ns) - + static_cast(rhs.fraction_ns); + result.Normalize(); + return result; +} + +/** + * @brief Offset a @ref Timestamp forward by a @ref TimeDelta. + * + * @param lhs The base timestamp. + * @param rhs The delta to add. + * + * @return A new @ref Timestamp equal to `lhs + rhs`, or an invalid + * timestamp if either operand is invalid or the result falls outside + * the representable range of @ref Timestamp. + */ +inline Timestamp operator+(Timestamp lhs, const TimeDelta& rhs) { + if (!lhs.IsValid() || !rhs.IsValid()) { + return Timestamp(); + } + + int64_t sec = static_cast(lhs.seconds) + rhs.seconds; + int64_t ns = static_cast(lhs.fraction_ns) + rhs.fraction_ns; + if (ns < 0) { + sec -= 1; + ns += 1000000000; + } else if (ns >= 1000000000) { + sec += ns / 1000000000; + ns %= 1000000000; + } + + if (sec < 0 || sec > static_cast(Timestamp::INVALID - 1)) { + return Timestamp(); + } else { + return Timestamp{static_cast(sec), static_cast(ns)}; + } +} + +/** + * @brief Offset a @ref Timestamp backwards by a @ref TimeDelta. + * + * @param lhs The base timestamp. + * @param rhs The delta to subtract. + * + * @return A new @ref Timestamp equal to `lhs - rhs`, or an invalid + * timestamp if either operand is invalid or the result falls outside + * the representable range of @ref Timestamp. + */ +inline Timestamp operator-(const Timestamp& lhs, const TimeDelta& rhs) { + if (!rhs.IsValid()) { + return Timestamp(); + } + + TimeDelta negated{-rhs.seconds, -rhs.fraction_ns}; + negated.Normalize(); + return lhs + negated; +} + +} // namespace messages +} // namespace fusion_engine +} // namespace point_one diff --git a/src/point_one/fusion_engine/messages/timestamp_test.cc b/src/point_one/fusion_engine/messages/timestamp_test.cc new file mode 100644 index 000000000..eeca47b35 --- /dev/null +++ b/src/point_one/fusion_engine/messages/timestamp_test.cc @@ -0,0 +1,348 @@ +#include "point_one/fusion_engine/messages/timestamp.h" + +#include +#include + +#include + +using namespace point_one::fusion_engine::messages; + +// GPS time for 2026/4/29 08:00:00 UTC (arbitrary reference point). +static constexpr uint32_t GPS_DATE_SEC = 1461484818; +static constexpr uint16_t GPS_DATE_WEEK = GPS_DATE_SEC / 604800; +static constexpr double GPS_DATE_TOW = GPS_DATE_SEC - GPS_DATE_WEEK * 604800.0; + +//////////////////////////////////////////////////////////////////////////////// +// Timestamp +//////////////////////////////////////////////////////////////////////////////// + +/******************************************************************************/ +TEST(TimestampConstruction, DefaultIsInvalid) { + Timestamp ts; + EXPECT_FALSE(ts.IsValid()); + EXPECT_FALSE(static_cast(ts)); +} + +/******************************************************************************/ +TEST(TimestampConstruction, ExplicitSecondsAndNs) { + Timestamp ts(10, 500000000); + EXPECT_TRUE(ts.IsValid()); + EXPECT_EQ(ts.seconds, 10u); + EXPECT_EQ(ts.fraction_ns, 500000000u); +} + +/******************************************************************************/ +TEST(TimestampConstruction, ZeroIsValid) { + Timestamp ts(0, 0); + EXPECT_TRUE(ts.IsValid()); +} + +/******************************************************************************/ +TEST(TimestampToSeconds, InvalidReturnsNaN) { + EXPECT_TRUE(std::isnan(Timestamp().ToSeconds())); +} + +/******************************************************************************/ +TEST(TimestampToSeconds, WholeSeconds) { + EXPECT_DOUBLE_EQ(Timestamp(10, 0).ToSeconds(), 10.0); +} + +/******************************************************************************/ +TEST(TimestampToSeconds, FractionalSeconds) { + EXPECT_NEAR(Timestamp(10, 500000000).ToSeconds(), 10.5, 1e-9); +} + +/******************************************************************************/ +TEST(TimestampToGPSWeekTOW, InvalidReturnsFalse) { + uint16_t week; + double tow; + EXPECT_FALSE(Timestamp().ToGPSWeekTOW(&week, &tow)); +} + +/******************************************************************************/ +TEST(TimestampToGPSWeekTOW, CorrectWeekAndTow) { + uint16_t week; + double tow; + ASSERT_TRUE(Timestamp(GPS_DATE_SEC, 0).ToGPSWeekTOW(&week, &tow)); + EXPECT_EQ(week, GPS_DATE_WEEK); + EXPECT_NEAR(tow, GPS_DATE_TOW, 1e-9); +} + +/******************************************************************************/ +TEST(TimestampToGPSWeekTOW, FractionalTow) { + uint16_t week; + double tow; + ASSERT_TRUE(Timestamp(GPS_DATE_SEC, 123000000).ToGPSWeekTOW(&week, &tow)); + EXPECT_NEAR(tow, GPS_DATE_TOW + 0.123, 1e-6); +} + +/******************************************************************************/ +TEST(TimestampToGPSWeekTOW, NullOutParams) { + // Should not crash when output pointers are null. + EXPECT_TRUE(Timestamp(GPS_DATE_SEC, 0).ToGPSWeekTOW(nullptr, nullptr)); +} + +/******************************************************************************/ +TEST(TimestampFromGPSTime, BasicConversion) { + Timestamp ts = Timestamp::FromGPSTime(GPS_DATE_WEEK, GPS_DATE_TOW); + EXPECT_TRUE(ts.IsValid()); + EXPECT_EQ(ts.seconds, GPS_DATE_SEC); + EXPECT_EQ(ts.fraction_ns, 0u); +} + +/******************************************************************************/ +TEST(TimestampFromGPSTime, RoundtripWithToGPSWeekTOW) { + uint16_t week; + double tow; + ASSERT_TRUE(Timestamp(GPS_DATE_SEC, 0).ToGPSWeekTOW(&week, &tow)); + Timestamp result = Timestamp::FromGPSTime(week, tow); + EXPECT_EQ(result.seconds, GPS_DATE_SEC); +} + +/******************************************************************************/ +TEST(TimestampFromGPSTime, FractionalTowPreserved) { + Timestamp ts = Timestamp::FromGPSTime(GPS_DATE_WEEK, GPS_DATE_TOW + 0.5); + EXPECT_TRUE(ts.IsValid()); + EXPECT_NEAR(ts.ToSeconds(), GPS_DATE_SEC + 0.5, 1e-6); +} + +/******************************************************************************/ +TEST(TimestampFromGPSTime, NaNTowReturnsInvalid) { + EXPECT_FALSE(Timestamp::FromGPSTime(GPS_DATE_WEEK, NAN).IsValid()); +} + +/******************************************************************************/ +TEST(TimestampFromGPSTime, InfiniteTowReturnsInvalid) { + EXPECT_FALSE(Timestamp::FromGPSTime(GPS_DATE_WEEK, + std::numeric_limits::infinity()) + .IsValid()); +} + +/******************************************************************************/ +TEST(TimestampFromGPSTime, ZeroTowIsValid) { + Timestamp ts = Timestamp::FromGPSTime(GPS_DATE_WEEK, 0.0); + EXPECT_TRUE(ts.IsValid()); + EXPECT_EQ(ts.seconds, GPS_DATE_WEEK * 604800u); + EXPECT_EQ(ts.fraction_ns, 0u); +} + +/******************************************************************************/ +TEST(TimestampFromGPSTime, NsRolloverNormalized) { + // tow with fractional part that rounds to exactly 1,000,000,000 ns should + // carry into seconds without leaving fraction_ns >= 1e9. + Timestamp ts = Timestamp::FromGPSTime(0, 1.9999999995); + EXPECT_TRUE(ts.IsValid()); + EXPECT_LT(ts.fraction_ns, 1000000000u); +} + +/******************************************************************************/ +TEST(TimestampSubtract, BothValidReturnsDelta) { + Timestamp a(10, 0); + Timestamp b(3, 0); + TimeDelta d = a - b; + EXPECT_TRUE(d.IsValid()); + EXPECT_NEAR(d.ToSeconds(), 7.0, 1e-9); +} + +/******************************************************************************/ +TEST(TimestampSubtract, InvalidOperandReturnInvalidDelta) { + EXPECT_FALSE((Timestamp(10, 0) - Timestamp()).IsValid()); + EXPECT_FALSE((Timestamp() - Timestamp(10, 0)).IsValid()); +} + +/******************************************************************************/ +TEST(TimestampAddDelta, PositiveDelta) { + Timestamp ts(10, 0); + TimeDelta d(5.5); + Timestamp result = ts + d; + EXPECT_TRUE(result.IsValid()); + EXPECT_EQ(result.seconds, 15u); + EXPECT_EQ(result.fraction_ns, 500000000u); +} + +/******************************************************************************/ +TEST(TimestampAddDelta, NegativeDelta) { + Timestamp ts(10, 0); + TimeDelta d(-5.5); + Timestamp result = ts + d; + EXPECT_TRUE(result.IsValid()); + EXPECT_EQ(result.seconds, 4u); + EXPECT_EQ(result.fraction_ns, 500000000u); +} + +/******************************************************************************/ +TEST(TimestampAddDelta, InvalidTimestampReturnsInvalid) { + EXPECT_FALSE((Timestamp() + TimeDelta(1, 0)).IsValid()); +} + +/******************************************************************************/ +TEST(TimestampAddDelta, InvalidDeltaReturnsInvalid) { + EXPECT_FALSE((Timestamp(10, 0) + TimeDelta()).IsValid()); +} + +/******************************************************************************/ +TEST(TimestampSubtractDelta, PositiveDelta) { + Timestamp ts(10, 0); + TimeDelta d(5.5); + Timestamp result = ts - d; + EXPECT_TRUE(result.IsValid()); + EXPECT_EQ(result.seconds, 4u); + EXPECT_EQ(result.fraction_ns, 500000000u); +} + +/******************************************************************************/ +TEST(TimestampSubtractDelta, NegativeDelta) { + Timestamp ts(10, 0); + TimeDelta d(-5.5); + Timestamp result = ts - d; + EXPECT_TRUE(result.IsValid()); + EXPECT_EQ(result.seconds, 15u); + EXPECT_EQ(result.fraction_ns, 500000000u); +} + +/******************************************************************************/ +TEST(TimestampSubtractDelta, InvalidDeltaReturnsInvalid) { + EXPECT_FALSE((Timestamp(10, 0) - TimeDelta()).IsValid()); +} + +//////////////////////////////////////////////////////////////////////////////// +// TimeDelta +//////////////////////////////////////////////////////////////////////////////// + +/******************************************************************************/ +TEST(TimeDeltaConstruction, DefaultIsInvalid) { + TimeDelta d; + EXPECT_FALSE(d.IsValid()); + EXPECT_FALSE(static_cast(d)); +} + +/******************************************************************************/ +TEST(TimeDeltaConstruction, SecondsAndNs) { + TimeDelta d(5, 500000000); + EXPECT_TRUE(d.IsValid()); + EXPECT_EQ(d.seconds, 5); + EXPECT_EQ(d.fraction_ns, 500000000); +} + +/******************************************************************************/ +TEST(TimeDeltaConstruction, NegativeSecondsAndNs) { + TimeDelta d(-5, -500000000); + EXPECT_TRUE(d.IsValid()); + EXPECT_EQ(d.seconds, -5); + EXPECT_EQ(d.fraction_ns, -500000000); +} + +/******************************************************************************/ +TEST(TimeDeltaConstruction, FromDouble) { + TimeDelta d(1.5); + EXPECT_TRUE(d.IsValid()); + EXPECT_EQ(d.seconds, 1); + EXPECT_EQ(d.fraction_ns, 500000000); +} + +/******************************************************************************/ +TEST(TimeDeltaConstruction, FromNegativeDouble) { + TimeDelta d(-1.5); + EXPECT_TRUE(d.IsValid()); + EXPECT_EQ(d.seconds, -1); + EXPECT_EQ(d.fraction_ns, -500000000); +} + +/******************************************************************************/ +TEST(TimeDeltaConstruction, FromNaNIsInvalid) { + EXPECT_FALSE(TimeDelta(NAN).IsValid()); +} + +/******************************************************************************/ +TEST(TimeDeltaToSeconds, InvalidReturnsNaN) { + EXPECT_TRUE(std::isnan(TimeDelta().ToSeconds())); +} + +/******************************************************************************/ +TEST(TimeDeltaToSeconds, PositiveValue) { + EXPECT_NEAR(TimeDelta(3, 250000000).ToSeconds(), 3.25, 1e-9); +} + +/******************************************************************************/ +TEST(TimeDeltaToSeconds, NegativeValue) { + EXPECT_NEAR(TimeDelta(-2, -500000000).ToSeconds(), -2.5, 1e-9); +} + +/******************************************************************************/ +TEST(TimeDeltaNormalize, OverflowNsCarriesIntoSeconds) { + TimeDelta d(0, 1500000000); + EXPECT_EQ(d.seconds, 1); + EXPECT_EQ(d.fraction_ns, 500000000); +} + +/******************************************************************************/ +TEST(TimeDeltaNormalize, NegativeOverflowNsCarriesIntoSeconds) { + TimeDelta d(0, -1500000000); + EXPECT_EQ(d.seconds, -1); + EXPECT_EQ(d.fraction_ns, -500000000); +} + +/******************************************************************************/ +TEST(TimeDeltaNormalize, MixedSignAligned) { + // {1, -200000000} → {0, 800000000} + TimeDelta d(1, -200000000); + EXPECT_EQ(d.seconds, 0); + EXPECT_EQ(d.fraction_ns, 800000000); +} + +/******************************************************************************/ +TEST(TimeDeltaNormalize, NegativeMixedSignAligned) { + // {-1, 200000000} → {0, -800000000} + TimeDelta d(-1, 200000000); + EXPECT_EQ(d.seconds, 0); + EXPECT_EQ(d.fraction_ns, -800000000); +} + +/******************************************************************************/ +TEST(TimeDeltaArithmetic, AddTwoDeltas) { + TimeDelta a(1, 500000000); + TimeDelta b(2, 600000000); + TimeDelta result = a + b; + EXPECT_TRUE(result.IsValid()); + EXPECT_EQ(result.seconds, 4); + EXPECT_EQ(result.fraction_ns, 100000000); +} + +/******************************************************************************/ +TEST(TimeDeltaArithmetic, SubtractTwoDeltas) { + TimeDelta a(5, 0); + TimeDelta b(3, 0); + TimeDelta result = a - b; + EXPECT_TRUE(result.IsValid()); + EXPECT_EQ(result.seconds, 2); + EXPECT_EQ(result.fraction_ns, 0); +} + +/******************************************************************************/ +TEST(TimeDeltaArithmetic, AddInvalidReturnsInvalid) { + TimeDelta valid(1, 0); + TimeDelta invalid; + EXPECT_FALSE((valid + invalid).IsValid()); + EXPECT_FALSE((invalid + valid).IsValid()); +} + +/******************************************************************************/ +TEST(TimeDeltaArithmetic, SubtractInvalidReturnsInvalid) { + TimeDelta valid(1, 0); + TimeDelta invalid; + EXPECT_FALSE((valid - invalid).IsValid()); +} + +/******************************************************************************/ +TEST(TimeDeltaArithmetic, CompoundAddAssign) { + TimeDelta d(1, 0); + d += TimeDelta(2, 0); + EXPECT_EQ(d.seconds, 3); +} + +/******************************************************************************/ +TEST(TimeDeltaArithmetic, CompoundSubtractAssign) { + TimeDelta d(5, 0); + d -= TimeDelta(2, 0); + EXPECT_EQ(d.seconds, 3); +} diff --git a/src/point_one/fusion_engine/parsers/fusion_engine_framer.cc b/src/point_one/fusion_engine/parsers/fusion_engine_framer.cc index 37bd0e74d..092e55254 100644 --- a/src/point_one/fusion_engine/parsers/fusion_engine_framer.cc +++ b/src/point_one/fusion_engine/parsers/fusion_engine_framer.cc @@ -5,7 +5,7 @@ #define P1_VMODULE_NAME fusion_engine_framer -#include "point_one/fusion_engine/parsers/fusion_engine_framer.h" +#include #include // For memmove() #if P1_HAVE_STD_OSTREAM @@ -14,8 +14,8 @@ # include #endif -#include "point_one/fusion_engine/common/logging.h" -#include "point_one/fusion_engine/messages/crc.h" +#include +#include using namespace point_one::fusion_engine::messages; using namespace point_one::fusion_engine::parsers; diff --git a/src/point_one/fusion_engine/parsers/fusion_engine_framer.h b/src/point_one/fusion_engine/parsers/fusion_engine_framer.h index 87ac8cfca..92fc7d1d6 100644 --- a/src/point_one/fusion_engine/parsers/fusion_engine_framer.h +++ b/src/point_one/fusion_engine/parsers/fusion_engine_framer.h @@ -5,7 +5,7 @@ #pragma once -#include "point_one/fusion_engine/common/portability.h" +#include #include // For size_t #include @@ -13,7 +13,7 @@ # include #endif // P1_HAVE_STD_FUNCTION -#include "point_one/fusion_engine/messages/defs.h" +#include namespace point_one { namespace fusion_engine { diff --git a/src/point_one/fusion_engine/utils/time_provider.cc b/src/point_one/fusion_engine/utils/time_provider.cc new file mode 100644 index 000000000..f0e91abad --- /dev/null +++ b/src/point_one/fusion_engine/utils/time_provider.cc @@ -0,0 +1,198 @@ +/**************************************************************************/ /** + * @brief Helper utility for converting between P1 and GPS time. + * @file + ******************************************************************************/ + +#define P1_VMODULE_NAME time_provider + +#include "point_one/fusion_engine/utils/time_provider.h" + +#include +#include +#include + +#include "point_one/fusion_engine/common/logging.h" + +using namespace point_one::fusion_engine::messages; +using namespace point_one::fusion_engine::utils; + +/******************************************************************************/ +class P1TimeFormat { + public: + const Timestamp time_; + + explicit P1TimeFormat(const Timestamp& time) : time_(time) {} + + inline friend std::ostream& operator<<(std::ostream& stream, + const P1TimeFormat& helper) { + if (helper.time_) { + stream << std::fixed << std::setprecision(9) << helper.time_.ToSeconds(); + } else { + stream << ""; + } + return stream; + } +}; + +/******************************************************************************/ +class GPSTimeFormat { + public: + const Timestamp time_; + + explicit GPSTimeFormat(const Timestamp& time) : time_(time) {} + + inline friend std::ostream& operator<<(std::ostream& stream, + const GPSTimeFormat& helper) { + uint16_t week_number = 0; + double tow_sec = NAN; + if (helper.time_.ToGPSWeekTOW(&week_number, &tow_sec)) { + stream << "Week " << week_number << ", TOW " << std::fixed + << std::setprecision(9) << tow_sec; + } else { + stream << ""; + } + return stream; + } +}; + +/******************************************************************************/ +void TimeProvider::Reset() { + VLOG(1) << "Resetting."; + *this = TimeProvider(); +} + +/******************************************************************************/ +void TimeProvider::HandleMessage(const MessageHeader& header, + const void* payload) { + if (header.message_type == MessageType::POSE) { + auto& message = *reinterpret_cast(payload); + + // Sanity check for duplicate or backwards timestamps. In practice this + // should not happen normally unless the device was reset. If time jumps + // backward, we'll assume it was a reset and store the new time. If we get a + // duplicate timestamp, we'll ignore it as a possible error. + if (current_p1_time_ && message.p1_time) { + double dt_sec = (message.p1_time - current_p1_time_).ToSeconds(); + if (dt_sec < -1e-3) { + LOG(WARNING) << "Backwards P1 time jump detected. Did the device " + "restart? [prev=" + << P1TimeFormat(current_p1_time_) + << ", current=" << P1TimeFormat(message.p1_time) + << ", dt=" << std::fixed << std::setprecision(2) << dt_sec + << " sec]"; + Reset(); + } else if (dt_sec < 1e-3) { + LOG(WARNING) << "Duplicate P1 timestamp detected. Ignoring. [prev=" + << P1TimeFormat(current_p1_time_) + << ", current=" << P1TimeFormat(message.p1_time) + << ", dt=" << std::fixed << std::setprecision(2) << dt_sec + << " sec]"; + return; + } + } + + // Store the current and previous P1/GPS times, and use them to convert + // to/from P1 or GPS time by interpolating (or extrapolating as needed). + // + // Note: If we had GPS time and the incoming message no longer does, we will + // no longer be able to convert P1<->GPS time. + prev_p1_time_ = current_p1_time_; + prev_gps_time_ = current_gps_time_; + current_p1_time_ = message.p1_time; + current_gps_time_ = message.gps_time; + + if (VLOG_IS_ON(1)) { + VLOG(1) << "Received time update (" << header.message_type + << " message) at:"; + VLOG(1) << " P1: " << P1TimeFormat(current_p1_time_); + VLOG(1) << " GPS: " << GPSTimeFormat(current_gps_time_); + if (current_p1_time_ && current_gps_time_ && prev_p1_time_ && + prev_gps_time_) { + VLOG(1) << " P1/GPS: " << std::fixed << std::setprecision(9) + << ((current_p1_time_ - prev_p1_time_).ToSeconds() / + (current_gps_time_ - prev_gps_time_).ToSeconds()) + << " sec/sec"; + } else { + VLOG(1) << " P1/GPS: "; + } + } + } +} + +/******************************************************************************/ +Timestamp TimeProvider::P1ToGPS(const Timestamp& p1_time) const { + if (!p1_time.IsValid()) { + VLOG(2) << "Cannot convert invalid P1 time to GPS time."; + return Timestamp(); + } else if (!current_p1_time_.IsValid() || !current_gps_time_.IsValid()) { + VLOG(2) << "P1/GPS relationship not known. Cannot convert P1 " + << P1TimeFormat(p1_time) << " to GPS time."; + return Timestamp(); + } + + // If we have both P1 and GPS time from the previous update, interpolate + // (or extrapolate) between the previous update and the current one for the + // most accurate result. + Timestamp gps_time; + if (prev_p1_time_.IsValid() && prev_gps_time_.IsValid()) { + double elapsed_p1_sec = (current_p1_time_ - prev_p1_time_).ToSeconds(); + double elapsed_gps_sec = (current_gps_time_ - prev_gps_time_).ToSeconds(); + double delta_p1_sec = (p1_time - prev_p1_time_).ToSeconds(); + double delta_gps_sec = elapsed_gps_sec * delta_p1_sec / elapsed_p1_sec; + int32_t int_sec = static_cast(delta_gps_sec); + TimeDelta delta_gps(int_sec, + static_cast((delta_gps_sec - int_sec) * 1e9)); + gps_time = prev_gps_time_ + delta_gps; + } + // Otherwise, use the current P1/GPS time offset with no interpolation. This + // will be less accurate since it cannot account for drift between P1 and GPS + // time, but for most purposes it will be fine as long as current_*_time_ is + // recent. + else { + double offset_sec = (current_gps_time_ - current_p1_time_).ToSeconds(); + gps_time = p1_time + TimeDelta(offset_sec); + } + + VLOG(2) << "Converted P1 " << P1TimeFormat(p1_time) << " to GPS " + << GPSTimeFormat(gps_time); + return gps_time; +} + +/******************************************************************************/ +Timestamp TimeProvider::GPSToP1(const Timestamp& gps_time) const { + if (!gps_time.IsValid()) { + VLOG(2) << "Cannot convert invalid GPS time to P1 time."; + return Timestamp(); + } else if (!current_p1_time_.IsValid() || !current_gps_time_.IsValid()) { + VLOG(2) << "P1/GPS relationship not known. Cannot convert GPS " + << GPSTimeFormat(gps_time) << " to P1 time."; + return Timestamp(); + } + + // If we have both P1 and GPS time from the previous update, interpolate + // (or extrapolate) between the previous update and the current one for the + // most accurate result. + Timestamp p1_time; + if (prev_gps_time_.IsValid() && prev_p1_time_.IsValid()) { + double elapsed_p1_sec = (current_p1_time_ - prev_p1_time_).ToSeconds(); + double elapsed_gps_sec = (current_gps_time_ - prev_gps_time_).ToSeconds(); + double delta_gps_sec = (gps_time - prev_gps_time_).ToSeconds(); + double delta_p1_sec = elapsed_p1_sec * delta_gps_sec / elapsed_gps_sec; + int32_t int_sec = static_cast(delta_p1_sec); + TimeDelta delta_p1(int_sec, + static_cast((delta_p1_sec - int_sec) * 1e9)); + p1_time = prev_p1_time_ + delta_p1; + } + // Otherwise, use the current P1/GPS time offset with no interpolation. This + // will be less accurate since it cannot account for drift between P1 and GPS + // time, but for most purposes it will be fine as long as current_*_time_ is + // recent. + else { + double offset_sec = (current_p1_time_ - current_gps_time_).ToSeconds(); + p1_time = gps_time + TimeDelta(offset_sec); + } + + VLOG(2) << "Converted GPS " << GPSTimeFormat(gps_time) << " to P1 " + << P1TimeFormat(p1_time); + return p1_time; +} diff --git a/src/point_one/fusion_engine/utils/time_provider.h b/src/point_one/fusion_engine/utils/time_provider.h new file mode 100644 index 000000000..22f8fae11 --- /dev/null +++ b/src/point_one/fusion_engine/utils/time_provider.h @@ -0,0 +1,78 @@ +/**************************************************************************/ /** + * @brief Helper utility for converting between P1 and GPS time. + * @file + ******************************************************************************/ + +#pragma once + +#include // For P1_EXPORT +#include + +namespace point_one { +namespace fusion_engine { +namespace utils { + +// Suppress MSVC C4251: private Timestamp members don't need DLL interface since +// they are a POD type fully defined in the header and not directly accessible +// by clients. +#ifdef _MSC_VER +# pragma warning(push) +# pragma warning(disable : 4251) +#endif + +/** + * @brief Utility for converting between P1 and GPS time. + */ +class P1_EXPORT TimeProvider { + public: + TimeProvider() = default; + + /** + * @brief Reset all known time relationships. + */ + void Reset(); + + /** + * @brief Learn time relationships from incoming FusionEngine messages. + * + * @param header The message header. + * @param payload The message payload. + */ + void HandleMessage(const messages::MessageHeader& header, + const void* payload); + + /** + * @brief Convert a P1 timestamp to GPS time. + * + * @param p1_time The P1 time to convert. + * + * @return The resulting GPS time, or an invalid timestamp if the time could + * not be converted. + */ + messages::Timestamp P1ToGPS(const messages::Timestamp& p1_time) const; + + /** + * @brief Convert a GPS timestamp to P1 time. + * + * @param gps_time The GPS time to convert. + * + * @return The resulting P1 time, or an invalid timestamp if the time could + * not be converted. + */ + messages::Timestamp GPSToP1(const messages::Timestamp& gps_time) const; + + private: + messages::Timestamp current_p1_time_; + messages::Timestamp current_gps_time_; + + messages::Timestamp prev_p1_time_; + messages::Timestamp prev_gps_time_; +}; + +#ifdef _MSC_VER +# pragma warning(pop) +#endif + +} // namespace utils +} // namespace fusion_engine +} // namespace point_one diff --git a/src/point_one/fusion_engine/utils/time_provider_test.cc b/src/point_one/fusion_engine/utils/time_provider_test.cc new file mode 100644 index 000000000..b017edbc2 --- /dev/null +++ b/src/point_one/fusion_engine/utils/time_provider_test.cc @@ -0,0 +1,283 @@ +#include "point_one/fusion_engine/utils/time_provider.h" + +#include + +#include "point_one/fusion_engine/messages/core.h" + +using namespace point_one::fusion_engine::messages; +using namespace point_one::fusion_engine::utils; + +// GPS time for 2026/4/29 08:00:00 UTC (arbitrary reference point). +static constexpr uint32_t GPS_DATE_SEC = 1461484818; + +static MessageHeader MakePoseHeader() { + MessageHeader header; + header.message_type = MessageType::POSE; + return header; +} + +static PoseMessage MakePose(double p1_sec, double gps_sec) { + PoseMessage msg; + msg.p1_time = + TimeDelta(p1_sec).IsValid() + ? Timestamp(static_cast(p1_sec), + static_cast( + (p1_sec - static_cast(p1_sec)) * 1e9)) + : Timestamp(); + msg.gps_time = Timestamp( + static_cast(gps_sec), + static_cast((gps_sec - static_cast(gps_sec)) * 1e9)); + return msg; +} + +// Feed one pose message into a TimeProvider. +static void Feed(TimeProvider& tp, double p1_sec, double gps_sec) { + auto header = MakePoseHeader(); + auto msg = MakePose(p1_sec, gps_sec); + tp.HandleMessage(header, &msg); +} + +//////////////////////////////////////////////////////////////////////////////// +// HandleMessage +//////////////////////////////////////////////////////////////////////////////// + +/******************************************************************************/ +TEST(HandleMessage, IgnoresNonPoseMessage) { + TimeProvider tp; + MessageHeader header; + header.message_type = MessageType::INVALID; + PoseMessage msg = MakePose(10.0, GPS_DATE_SEC); + tp.HandleMessage(header, &msg); + EXPECT_FALSE(tp.P1ToGPS(Timestamp(10, 0)).IsValid()); +} + +/******************************************************************************/ +TEST(HandleMessage, StoresFirstUpdate) { + TimeProvider tp; + Feed(tp, 10.0, GPS_DATE_SEC); + // A conversion using the stored reference should succeed. + EXPECT_TRUE(tp.P1ToGPS(Timestamp(10, 0)).IsValid()); +} + +/******************************************************************************/ +TEST(HandleMessage, AdvancesPrevOnSecondUpdate) { + TimeProvider tp; + Feed(tp, 10.0, GPS_DATE_SEC); + Feed(tp, 11.0, GPS_DATE_SEC + 1); + // With two references, interpolation is active; midpoint should convert. + Timestamp gps = tp.P1ToGPS(Timestamp(10, 500000000)); + EXPECT_TRUE(gps.IsValid()); + EXPECT_NEAR(gps.ToSeconds(), GPS_DATE_SEC + 0.5, 1e-3); +} + +//////////////////////////////////////////////////////////////////////////////// +// Backwards / duplicate timestamp handling +//////////////////////////////////////////////////////////////////////////////// + +/******************************************************************************/ +TEST(HandleMessage, BackwardsTimestampResetsState) { + TimeProvider tp; + Feed(tp, 10.0, GPS_DATE_SEC); + Feed(tp, 11.0, GPS_DATE_SEC + 1); + Feed(tp, 5.0, GPS_DATE_SEC - 5); + // After reset only the new reference is known; prev is cleared. + // Conversion at the new reference point should succeed. + Timestamp gps = tp.P1ToGPS(Timestamp(5, 0)); + EXPECT_TRUE(gps.IsValid()); + EXPECT_NEAR(gps.ToSeconds(), GPS_DATE_SEC - 5.0, 1e-3); + // A query before the new reference extrapolates from a single point. + Timestamp gps2 = tp.P1ToGPS(Timestamp(7, 0)); + EXPECT_TRUE(gps2.IsValid()); + EXPECT_NEAR(gps2.ToSeconds(), GPS_DATE_SEC - 3.0, 1e-3); +} + +/******************************************************************************/ +TEST(HandleMessage, BackwardsTimestampClearsPrev) { + TimeProvider tp; + Feed(tp, 10.0, GPS_DATE_SEC); + Feed(tp, 11.0, GPS_DATE_SEC + 1); + Feed(tp, 5.0, GPS_DATE_SEC - 5); + // No prev means GPSToP1 falls back to single-reference mode. + Timestamp p1 = tp.GPSToP1(Timestamp(GPS_DATE_SEC - 5, 0)); + EXPECT_TRUE(p1.IsValid()); + EXPECT_NEAR(p1.ToSeconds(), 5.0, 1e-3); +} + +/******************************************************************************/ +TEST(HandleMessage, ExactDuplicateIsIgnored) { + TimeProvider tp; + Feed(tp, 10.0, GPS_DATE_SEC); + // Feed the same P1 time again; should be silently dropped. + Feed(tp, 10.0, GPS_DATE_SEC); + // State should reflect only a single reference (no prev). + // Conversion should still work using the first reference. + Timestamp gps = tp.P1ToGPS(Timestamp(12, 0)); + EXPECT_TRUE(gps.IsValid()); + EXPECT_NEAR(gps.ToSeconds(), GPS_DATE_SEC + 2.0, 1e-3); +} + +/******************************************************************************/ +TEST(HandleMessage, NearDuplicateBelowThresholdIsIgnored) { + // 0.5 ms forward jump — below the 1 ms duplicate threshold. + TimeProvider tp; + Feed(tp, 10.0, GPS_DATE_SEC); + Feed(tp, 10.0005, GPS_DATE_SEC + 0.0005); + // Prev should still be invalid (second message was dropped). + // Single-reference conversion should still be correct. + Timestamp gps = tp.P1ToGPS(Timestamp(12, 0)); + EXPECT_TRUE(gps.IsValid()); + EXPECT_NEAR(gps.ToSeconds(), GPS_DATE_SEC + 2.0, 1e-3); +} + +/******************************************************************************/ +TEST(HandleMessage, NearDuplicateAboveThresholdIsAccepted) { + // 2 ms forward jump — above the 1 ms threshold, should be stored. + TimeProvider tp; + Feed(tp, 10.0, GPS_DATE_SEC); + Feed(tp, 10.002, GPS_DATE_SEC + 0.002); + // With two references the midpoint should interpolate correctly. + Timestamp gps = tp.P1ToGPS(Timestamp(10, 1000000)); + EXPECT_TRUE(gps.IsValid()); + EXPECT_NEAR(gps.ToSeconds(), GPS_DATE_SEC + 0.001, 1e-4); +} + +//////////////////////////////////////////////////////////////////////////////// +// P1ToGPS +//////////////////////////////////////////////////////////////////////////////// + +/******************************************************************************/ +TEST(P1ToGPS, InvalidP1ReturnsInvalid) { + TimeProvider tp; + Feed(tp, 10.0, GPS_DATE_SEC); + EXPECT_FALSE(tp.P1ToGPS(Timestamp()).IsValid()); +} + +/******************************************************************************/ +TEST(P1ToGPS, NoReferenceReturnsInvalid) { + TimeProvider tp; + EXPECT_FALSE(tp.P1ToGPS(Timestamp(10, 0)).IsValid()); +} + +/******************************************************************************/ +TEST(P1ToGPS, SingleReferenceStraightOffset) { + TimeProvider tp; + Feed(tp, 10.0, GPS_DATE_SEC); + Timestamp gps = tp.P1ToGPS(Timestamp(12, 0)); + EXPECT_TRUE(gps.IsValid()); + EXPECT_NEAR(gps.ToSeconds(), GPS_DATE_SEC + 2.0, 1e-6); +} + +/******************************************************************************/ +TEST(P1ToGPS, TwoReferencesInterpolation) { + TimeProvider tp; + Feed(tp, 10.0, GPS_DATE_SEC); + Feed(tp, 20.0, GPS_DATE_SEC + 10); + Timestamp gps = tp.P1ToGPS(Timestamp(15, 0)); + EXPECT_TRUE(gps.IsValid()); + EXPECT_NEAR(gps.ToSeconds(), GPS_DATE_SEC + 5.0, 1e-3); +} + +/******************************************************************************/ +TEST(P1ToGPS, TwoReferencesExtrapolation) { + TimeProvider tp; + Feed(tp, 10.0, GPS_DATE_SEC); + Feed(tp, 20.0, GPS_DATE_SEC + 10); + Timestamp gps = tp.P1ToGPS(Timestamp(25, 0)); + EXPECT_TRUE(gps.IsValid()); + EXPECT_NEAR(gps.ToSeconds(), GPS_DATE_SEC + 15.0, 1e-3); +} + +/******************************************************************************/ +TEST(P1ToGPS, InterpolationWithDrift) { + // P1 runs slightly fast: 10 P1-sec == 10.001 GPS-sec. + TimeProvider tp; + Feed(tp, 10.0, GPS_DATE_SEC); + Feed(tp, 20.0, GPS_DATE_SEC + 10.001); + Timestamp gps = tp.P1ToGPS(Timestamp(15, 0)); + EXPECT_TRUE(gps.IsValid()); + EXPECT_NEAR(gps.ToSeconds(), GPS_DATE_SEC + 5.0005, 1e-3); +} + +//////////////////////////////////////////////////////////////////////////////// +// GPSToP1 +//////////////////////////////////////////////////////////////////////////////// + +/******************************************************************************/ +TEST(GPSToP1, InvalidGPSReturnsInvalid) { + TimeProvider tp; + Feed(tp, 10.0, GPS_DATE_SEC); + EXPECT_FALSE(tp.GPSToP1(Timestamp()).IsValid()); +} + +/******************************************************************************/ +TEST(GPSToP1, NoReferenceReturnsInvalid) { + TimeProvider tp; + EXPECT_FALSE(tp.GPSToP1(Timestamp(GPS_DATE_SEC, 0)).IsValid()); +} + +/******************************************************************************/ +TEST(GPSToP1, SingleReferenceStraightOffset) { + TimeProvider tp; + Feed(tp, 10.0, GPS_DATE_SEC); + Timestamp p1 = tp.GPSToP1(Timestamp(GPS_DATE_SEC + 2, 0)); + EXPECT_TRUE(p1.IsValid()); + EXPECT_NEAR(p1.ToSeconds(), 12.0, 1e-6); +} + +/******************************************************************************/ +TEST(GPSToP1, TwoReferencesInterpolation) { + TimeProvider tp; + Feed(tp, 10.0, GPS_DATE_SEC); + Feed(tp, 20.0, GPS_DATE_SEC + 10); + Timestamp p1 = tp.GPSToP1(Timestamp(GPS_DATE_SEC + 5, 0)); + EXPECT_TRUE(p1.IsValid()); + EXPECT_NEAR(p1.ToSeconds(), 15.0, 1e-3); +} + +/******************************************************************************/ +TEST(GPSToP1, TwoReferencesExtrapolation) { + TimeProvider tp; + Feed(tp, 10.0, GPS_DATE_SEC); + Feed(tp, 20.0, GPS_DATE_SEC + 10); + Timestamp p1 = tp.GPSToP1(Timestamp(GPS_DATE_SEC + 15, 0)); + EXPECT_TRUE(p1.IsValid()); + EXPECT_NEAR(p1.ToSeconds(), 25.0, 1e-3); +} + +/******************************************************************************/ +TEST(GPSToP1, RoundtripP1ToGPSToP1) { + TimeProvider tp; + Feed(tp, 10.0, GPS_DATE_SEC); + Feed(tp, 20.0, GPS_DATE_SEC + 10); + Timestamp original(14, 500000000); + Timestamp gps = tp.P1ToGPS(original); + ASSERT_TRUE(gps.IsValid()); + Timestamp recovered = tp.GPSToP1(gps); + ASSERT_TRUE(recovered.IsValid()); + EXPECT_NEAR(recovered.ToSeconds(), original.ToSeconds(), 1e-6); +} + +//////////////////////////////////////////////////////////////////////////////// +// Reset +//////////////////////////////////////////////////////////////////////////////// + +/******************************************************************************/ +TEST(Reset, ClearsAllState) { + TimeProvider tp; + Feed(tp, 10.0, GPS_DATE_SEC); + Feed(tp, 20.0, GPS_DATE_SEC + 10); + tp.Reset(); + EXPECT_FALSE(tp.P1ToGPS(Timestamp(10, 0)).IsValid()); + EXPECT_FALSE(tp.GPSToP1(Timestamp(GPS_DATE_SEC, 0)).IsValid()); +} + +/******************************************************************************/ +TEST(Reset, AllowsReuse) { + TimeProvider tp; + Feed(tp, 10.0, GPS_DATE_SEC); + tp.Reset(); + Feed(tp, 5.0, GPS_DATE_SEC + 100); + Timestamp gps = tp.P1ToGPS(Timestamp(7, 0)); + EXPECT_TRUE(gps.IsValid()); + EXPECT_NEAR(gps.ToSeconds(), GPS_DATE_SEC + 102.0, 1e-3); +}