Added P1/GPS time conversion support.

.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:

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",
+    ],
+)

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)

README.md

@@ -1,5 +1,6 @@
 <table>
 <tr><td>Build Status</td><td><img src="https://img.shields.io/github/actions/workflow/status/PointOneNav/fusion-engine-client/release_build.yml?label=Build+Status"/></td></tr>
+<tr><td>C++ Support</td><td><img src="https://img.shields.io/github/actions/workflow/status/PointOneNav/fusion-engine-client/release_build.yml?job=Bazel+Build+(ubuntu-latest,+gcc,+11,+6.5.0,+c%2B%2B11)&label=c%2B%2B11"/> <img src="https://img.shields.io/github/actions/workflow/status/PointOneNav/fusion-engine-client/release_build.yml?job=Bazel+Build+(ubuntu-latest,+gcc,+11,+6.5.0,+c%2B%2B14)&label=c%2B%2B14"/> <img src="https://img.shields.io/github/actions/workflow/status/PointOneNav/fusion-engine-client/release_build.yml?job=Bazel+Build+(ubuntu-latest,+gcc,+11,+6.5.0,+c%2B%2B17)&label=c%2B%2B17"/> <img src="https://img.shields.io/github/actions/workflow/status/PointOneNav/fusion-engine-client/release_build.yml?job=Bazel+Build+(ubuntu-latest,+gcc,+11,+6.5.0,+c%2B%2B20)&label=c%2B%2B20"/> <img src="https://img.shields.io/github/actions/workflow/status/PointOneNav/fusion-engine-client/release_build.yml?job=Bazel+Build+(ubuntu-latest,+gcc,+11,+6.5.0,+c%2B%2B23)&label=c%2B%2B23"/></td></tr>
 <tr><td>Python Support</td><td><img src="https://img.shields.io/github/actions/workflow/status/PointOneNav/fusion-engine-client/release_build.yml?job=Python+Unit+Tests+(3.8)&label=3.8"/> <img src="https://img.shields.io/github/actions/workflow/status/PointOneNav/fusion-engine-client/release_build.yml?job=Python+Unit+Tests+(3.9)&label=3.9"/> <img src="https://img.shields.io/github/actions/workflow/status/PointOneNav/fusion-engine-client/release_build.yml?job=Python+Unit+Tests+(3.10)&label=3.10"/> <img src="https://img.shields.io/github/actions/workflow/status/PointOneNav/fusion-engine-client/release_build.yml?job=Python+Unit+Tests+(3.11)&label=3.11"/> <img src="https://img.shields.io/github/actions/workflow/status/PointOneNav/fusion-engine-client/release_build.yml?job=Python+Unit+Tests+(3.12)&label=3.12"/> <img src="https://img.shields.io/github/actions/workflow/status/PointOneNav/fusion-engine-client/release_build.yml?job=Python+Unit+Tests+(3.13)&label=3.13"/></td></tr>
 <tr><td>CodeQL</td><td><img src="https://img.shields.io/github/actions/workflow/status/PointOneNav/fusion-engine-client/codeql.yml?label=CodeQL"/></td></tr>
 </table>

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"],
+)

examples/.bazelversion

@@ -0,0 +1 @@
+6.5.0

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",
-    ]
+    ],
 )

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)

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",
+    ],
+)

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",
+    ],
+)

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)

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 <cmath>
+#include <cstdio>
+
+#include <point_one/fusion_engine/messages/core.h>
+#include <point_one/fusion_engine/utils/time_provider.h>
+
+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("<invalid>\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;
+}