From 8d8f247207f6cba482eee016021e650308e3145b Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Wed, 29 Apr 2026 08:51:32 -0400 Subject: [PATCH 01/52] Moved Timestamp struct definition to its own header. --- BUILD | 1 + src/point_one/fusion_engine/messages/defs.h | 21 +--------- .../fusion_engine/messages/timestamp.h | 40 +++++++++++++++++++ 3 files changed, 42 insertions(+), 20 deletions(-) create mode 100644 src/point_one/fusion_engine/messages/timestamp.h diff --git a/BUILD b/BUILD index 7189a67c9..249f6b686 100644 --- a/BUILD +++ b/BUILD @@ -56,6 +56,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", diff --git a/src/point_one/fusion_engine/messages/defs.h b/src/point_one/fusion_engine/messages/defs.h index 58de69e99..229fa1460 100644 --- a/src/point_one/fusion_engine/messages/defs.h +++ b/src/point_one/fusion_engine/messages/defs.h @@ -11,6 +11,7 @@ #include "point_one/fusion_engine/common/portability.h" #include "point_one/fusion_engine/messages/signal_defs.h" +#include "point_one/fusion_engine/messages/timestamp.h" namespace point_one { namespace fusion_engine { @@ -586,26 +587,6 @@ inline p1_ostream& operator<<(p1_ostream& stream, SolutionType type) { /** @} */ -/** - * @brief Generic timestamp representation. - * - * 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 The header present at the beginning of every message. * @ingroup messages diff --git a/src/point_one/fusion_engine/messages/timestamp.h b/src/point_one/fusion_engine/messages/timestamp.h new file mode 100644 index 000000000..1b88c41d4 --- /dev/null +++ b/src/point_one/fusion_engine/messages/timestamp.h @@ -0,0 +1,40 @@ +/**************************************************************************/ /** + * @brief Point One FusionEngine timestamp support. + * @file + ******************************************************************************/ + +#pragma once + +#include // 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; +}; + +} // namespace messages +} // namespace fusion_engine +} // namespace point_one From cd80581c38896ec8de26dc51a9ae87b96a7b3e57 Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Wed, 29 Apr 2026 09:04:26 -0400 Subject: [PATCH 02/52] Added helper functions to Timestamp class. --- .../fusion_engine/messages/timestamp.h | 56 +++++++++++++++++++ 1 file changed, 56 insertions(+) diff --git a/src/point_one/fusion_engine/messages/timestamp.h b/src/point_one/fusion_engine/messages/timestamp.h index 1b88c41d4..3b474539e 100644 --- a/src/point_one/fusion_engine/messages/timestamp.h +++ b/src/point_one/fusion_engine/messages/timestamp.h @@ -33,6 +33,62 @@ struct P1_ALIGNAS(4) Timestamp { /** The fractional part of the second, expressed in nanoseconds. */ uint32_t fraction_ns = INVALID; + + /** + * @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 Get the timestamp value in seconds. + * + * @return The timestamp value (in seconds), or `NAN` if invalid. + */ + operator double() const { return ToSeconds(); } }; } // namespace messages From dffefabaf14c8297fb4c37017a13a553d0832a5e Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Wed, 29 Apr 2026 09:29:38 -0400 Subject: [PATCH 03/52] Added TimestampDelta helper class. --- .../fusion_engine/messages/timestamp.h | 280 ++++++++++++++++++ 1 file changed, 280 insertions(+) diff --git a/src/point_one/fusion_engine/messages/timestamp.h b/src/point_one/fusion_engine/messages/timestamp.h index 3b474539e..f549b8879 100644 --- a/src/point_one/fusion_engine/messages/timestamp.h +++ b/src/point_one/fusion_engine/messages/timestamp.h @@ -91,6 +91,286 @@ struct P1_ALIGNAS(4) Timestamp { operator double() const { return ToSeconds(); } }; +/** + * @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, `TimestampDelta` 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 TimestampDelta { + 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. */ + TimestampDelta() = 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, `TimestampDelta(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. + */ + TimestampDelta(int32_t sec, int32_t ns) : seconds(sec), fraction_ns(ns) { + if (seconds != INVALID && fraction_ns != INVALID) { + 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 >= 1'000'000'000) { + seconds += fraction_ns / 1'000'000'000; + fraction_ns %= 1'000'000'000; + } else if (fraction_ns <= -1'000'000'000) { + seconds += fraction_ns / 1'000'000'000; + fraction_ns %= 1'000'000'000; + } + + // 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 -= 1'000'000'000; + } else if (fraction_ns < 0 && seconds > 0) { + seconds -= 1; + fraction_ns += 1'000'000'000; + } + } + + /** + * @brief Check if this is a valid delta. + * + * @return `true` if the delta is valid. + */ + operator bool() const { return IsValid(); } + + /** + * @brief Convert this delta to a floating-point number of seconds. + * + * @return The delta expressed as seconds, or `NAN` if the delta is + * invalid. + */ + operator double() const { return ToSeconds(); } + + /** + * @brief Add another @ref TimestampDelta 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. + */ + TimestampDelta& operator+=(const TimestampDelta& 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 TimestampDelta 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. + */ + TimestampDelta& operator-=(const TimestampDelta& 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 TimestampDelta 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 TimestampDelta operator+(TimestampDelta lhs, + const TimestampDelta& rhs) { + lhs += rhs; + return lhs; + } + + /** + * @brief Subtract one @ref TimestampDelta 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 TimestampDelta operator-(TimestampDelta lhs, + const TimestampDelta& rhs) { + lhs -= rhs; + return lhs; + } +}; + +/** + * @brief Compute the difference between two @ref Timestamp values as a + * @ref TimestampDelta. + * + * @param lhs The timestamp to subtract from. + * @param rhs The timestamp to subtract. + * + * @return A @ref TimestampDelta representing `lhs - rhs`, or an invalid + * delta if either operand is invalid. + */ +inline TimestampDelta operator-(const Timestamp& lhs, const Timestamp& rhs) { + if (!lhs.IsValid() || !rhs.IsValid()) { + return TimestampDelta(); + } + + TimestampDelta 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 TimestampDelta. + * + * @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 TimestampDelta& 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 += 1'000'000'000; + } else if (ns >= 1'000'000'000) { + sec += ns / 1'000'000'000; + ns %= 1'000'000'000; + } + + 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 TimestampDelta. + * + * @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 TimestampDelta& rhs) { + if (!rhs.IsValid()) { + return Timestamp(); + } + + TimestampDelta negated{-rhs.seconds, -rhs.fraction_ns}; + negated.Normalize(); + return lhs + negated; +} + } // namespace messages } // namespace fusion_engine } // namespace point_one From a35f664c382b0173b1fab97cc05931eeaf6af908 Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Wed, 29 Apr 2026 09:52:54 -0400 Subject: [PATCH 04/52] Added example TimeProvider class to convert between P1 and GPS time. --- examples/common/BUILD | 13 +++++ examples/common/CMakeLists.txt | 3 ++ examples/common/time_provider.cc | 87 ++++++++++++++++++++++++++++++++ examples/common/time_provider.h | 40 +++++++++++++++ 4 files changed, 143 insertions(+) create mode 100644 examples/common/time_provider.cc create mode 100644 examples/common/time_provider.h 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/common/CMakeLists.txt b/examples/common/CMakeLists.txt index 65a84632d..b9bb23b1c 100644 --- a/examples/common/CMakeLists.txt +++ b/examples/common/CMakeLists.txt @@ -1,2 +1,5 @@ add_library(print_message STATIC print_message.cc) target_link_libraries(print_message PUBLIC fusion_engine_client) + +add_library(time_provider STATIC time_provider.cc) +target_link_libraries(time_provider PUBLIC fusion_engine_client) diff --git a/examples/common/time_provider.cc b/examples/common/time_provider.cc new file mode 100644 index 000000000..e9cf6fdab --- /dev/null +++ b/examples/common/time_provider.cc @@ -0,0 +1,87 @@ +/**************************************************************************/ /** + * @brief Example utility for converting between P1 and GPS time. + * @file + ******************************************************************************/ + +#include "time_provider.h" + +using namespace point_one::fusion_engine::examples; +using namespace point_one::fusion_engine::messages; + +/******************************************************************************/ +void TimeProvider::Reset() { *this = TimeProvider(); } + +/******************************************************************************/ +void TimeProvider::HandleMessage(const MessageHeader& header, + const void* payload) { + if (header.message_type == MessageType::POSE) { + // Store the current and previous P1/GPS times, and use them to to convert + // to/from P1 or GPS time by interpolating. + // + // Note: If we had GPS time and the incoming message no longer does, we will + // no longer be able to convert P1<->GPS time. + auto& message = *reinterpret_cast(payload); + prev_p1_time_ = current_p1_time_; + prev_gps_time_ = current_gps_time_; + current_p1_time_ = message.p1_time; + current_gps_time_ = message.gps_time; + } +} + +/******************************************************************************/ +Timestamp TimeProvider::P1ToGPS(const Timestamp& p1_time) const { + if (!p1_time.IsValid() || !current_p1_time_.IsValid() || + !current_gps_time_.IsValid()) { + 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 (prev_p1_time_.IsValid() && prev_gps_time_.IsValid()) { + double elapsed_p1_sec = current_p1_time_ - prev_p1_time_; + double elapsed_gps_sec = current_gps_time_ - prev_gps_time_; + double delta_p1_sec = p1_time - prev_p1_time_; + double offset_sec = elapsed_gps_sec * delta_p1_sec / elapsed_p1_sec; + int32_t int_sec = static_cast(offset_sec); + TimestampDelta delta_gps(int_sec, + static_cast((offset_sec - int_sec) * 1e9)); + return 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 { + return p1_time + (current_gps_time_ - current_p1_time_); + } +} + +/******************************************************************************/ +Timestamp TimeProvider::GPSToP1(const Timestamp& gps_time) const { + if (!gps_time.IsValid() || !current_p1_time_.IsValid() || + !current_gps_time_.IsValid()) { + 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 (prev_gps_time_.IsValid() && prev_p1_time_.IsValid()) { + double elapsed_gps_sec = current_gps_time_ - prev_gps_time_; + double elapsed_p1_sec = current_p1_time_ - prev_p1_time_; + double delta_gps_sec = gps_time - prev_gps_time_; + double offset_sec = elapsed_p1_sec * delta_gps_sec / elapsed_gps_sec; + int32_t int_sec = static_cast(offset_sec); + TimestampDelta delta_p1(int_sec, + static_cast((offset_sec - int_sec) * 1e9)); + return 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 { + return gps_time + (current_p1_time_ - current_gps_time_); + } +} diff --git a/examples/common/time_provider.h b/examples/common/time_provider.h new file mode 100644 index 000000000..20511fc00 --- /dev/null +++ b/examples/common/time_provider.h @@ -0,0 +1,40 @@ +/**************************************************************************/ /** + * @brief Example utility for converting between P1 and GPS time. + * @file + ******************************************************************************/ + +#pragma once + +#include + +namespace point_one { +namespace fusion_engine { +namespace examples { + +/** + * @brief Utility for converting between P1 and GPS time. + */ +class TimeProvider { + public: + TimeProvider() = default; + + void Reset(); + + void HandleMessage(const messages::MessageHeader& header, + const void* payload); + + messages::Timestamp P1ToGPS(const messages::Timestamp& p1_time) const; + + 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_; +}; + +} // namespace examples +} // namespace fusion_engine +} // namespace point_one From fda3adc33dc91936905e87a480f4c442087c9c1e Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Wed, 29 Apr 2026 10:21:25 -0400 Subject: [PATCH 05/52] Added .bazelversion to examples/ workspace. --- examples/.bazelversion | 1 + 1 file changed, 1 insertion(+) create mode 100644 examples/.bazelversion 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 From 26af0e6b535e07cd9bb8024872eea2afbf9e57be Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Wed, 29 Apr 2026 10:21:43 -0400 Subject: [PATCH 06/52] Added missing example apps to Bazel filegroup. --- examples/BUILD | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/examples/BUILD b/examples/BUILD index ce0020e21..f5d6cfa21 100644 --- a/examples/BUILD +++ b/examples/BUILD @@ -5,9 +5,11 @@ filegroup( name = "examples", srcs = [ "//generate_data", + "//lband_decode", "//message_decode", + "//raw_message_decode", "//request_version", "//tcp_client", "//udp_client", - ] + ], ) From 2e12f5bfd16d23db9504c41f6e5c5753f56ca089 Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Wed, 29 Apr 2026 10:21:58 -0400 Subject: [PATCH 07/52] Added Timestamp::FromGPSTime() helper. --- .../fusion_engine/messages/timestamp.h | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/src/point_one/fusion_engine/messages/timestamp.h b/src/point_one/fusion_engine/messages/timestamp.h index f549b8879..957ef4a1b 100644 --- a/src/point_one/fusion_engine/messages/timestamp.h +++ b/src/point_one/fusion_engine/messages/timestamp.h @@ -89,6 +89,24 @@ struct P1_ALIGNAS(4) Timestamp { * @return The timestamp value (in seconds), or `NAN` if invalid. */ operator double() const { return ToSeconds(); } + + /** + * @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; + 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)); + return result; + } }; /** From 5501583b5e58b0b528000158d01d8d9ed0650778 Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Wed, 29 Apr 2026 11:30:06 -0400 Subject: [PATCH 08/52] Added C++ time conversion example app. --- examples/BUILD | 1 + examples/CMakeLists.txt | 1 + examples/convert_time/BUILD | 12 ++++ examples/convert_time/CMakeLists.txt | 3 + examples/convert_time/convert_time.cc | 99 +++++++++++++++++++++++++++ 5 files changed, 116 insertions(+) create mode 100644 examples/convert_time/BUILD create mode 100644 examples/convert_time/CMakeLists.txt create mode 100644 examples/convert_time/convert_time.cc diff --git a/examples/BUILD b/examples/BUILD index f5d6cfa21..d3a77e884 100644 --- a/examples/BUILD +++ b/examples/BUILD @@ -4,6 +4,7 @@ package(default_visibility = ["//visibility:public"]) filegroup( name = "examples", srcs = [ + "//convert_time", "//generate_data", "//lband_decode", "//message_decode", 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/convert_time/BUILD b/examples/convert_time/BUILD new file mode 100644 index 000000000..d5578c53d --- /dev/null +++ b/examples/convert_time/BUILD @@ -0,0 +1,12 @@ +package(default_visibility = ["//visibility:public"]) + +cc_binary( + name = "convert_time", + srcs = [ + "convert_time.cc", + ], + deps = [ + "//common:time_provider", + "@fusion_engine_client", + ], +) diff --git a/examples/convert_time/CMakeLists.txt b/examples/convert_time/CMakeLists.txt new file mode 100644 index 000000000..b6987f9a9 --- /dev/null +++ b/examples/convert_time/CMakeLists.txt @@ -0,0 +1,3 @@ +add_executable(convert_time convert_time.cc) +target_link_libraries(convert_time PUBLIC fusion_engine_client) +target_link_libraries(convert_time PUBLIC time_provider) diff --git a/examples/convert_time/convert_time.cc b/examples/convert_time/convert_time.cc new file mode 100644 index 000000000..94e50e8b7 --- /dev/null +++ b/examples/convert_time/convert_time.cc @@ -0,0 +1,99 @@ +/**************************************************************************/ /** + * @brief Use the example `TimeProvider` class to convert between P1 and GPS + * time. + * @file + ******************************************************************************/ + +#include + +#include "../common/time_provider.h" + +using namespace point_one::fusion_engine::examples; +using namespace point_one::fusion_engine::messages; + +/******************************************************************************/ +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, 999'000'000}; + 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); + time_provider.HandleMessage(pose_header, &pose1); + printf("First pose message:\n"); + PrintTimes(pose1.p1_time, pose1.gps_time); + + 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, 100'000'500}; + 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, 100'001'000}; + pose2.gps_time = Timestamp::FromGPSTime(2416, 288018.1); + time_provider.HandleMessage(pose_header, &pose2); + printf("Second pose message:\n"); + PrintTimes(pose2.p1_time, pose2.gps_time); + + 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, 100'001'500}; + printf("IMU measurement 3:\n"); + PrintIMUMeasurement(time_provider, imu3); + + return 0; +} From 780f1708f362371f4d55b51ca421c2d4fcfc5ddb Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Wed, 29 Apr 2026 11:40:27 -0400 Subject: [PATCH 09/52] Moved TimeProvider class into library utils/ directory. --- BUILD | 15 +++++++++++++++ CMakeLists.txt | 1 + examples/common/CMakeLists.txt | 3 --- examples/convert_time/BUILD | 1 - examples/convert_time/CMakeLists.txt | 1 - examples/convert_time/convert_time.cc | 5 ++--- .../fusion_engine/utils}/time_provider.cc | 6 +++--- .../fusion_engine/utils}/time_provider.h | 6 +++--- 8 files changed, 24 insertions(+), 14 deletions(-) rename {examples/common => src/point_one/fusion_engine/utils}/time_provider.cc (95%) rename {examples/common => src/point_one/fusion_engine/utils}/time_provider.h (89%) diff --git a/BUILD b/BUILD index 249f6b686..f70e30d71 100644 --- a/BUILD +++ b/BUILD @@ -8,6 +8,7 @@ cc_library( ":messages", ":parsers", ":rtcm", + ":utils", ], ) @@ -161,3 +162,17 @@ 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 = [ + ":core_headers", + ], +) 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/examples/common/CMakeLists.txt b/examples/common/CMakeLists.txt index b9bb23b1c..65a84632d 100644 --- a/examples/common/CMakeLists.txt +++ b/examples/common/CMakeLists.txt @@ -1,5 +1,2 @@ add_library(print_message STATIC print_message.cc) target_link_libraries(print_message PUBLIC fusion_engine_client) - -add_library(time_provider STATIC time_provider.cc) -target_link_libraries(time_provider PUBLIC fusion_engine_client) diff --git a/examples/convert_time/BUILD b/examples/convert_time/BUILD index d5578c53d..5cc4e6ab2 100644 --- a/examples/convert_time/BUILD +++ b/examples/convert_time/BUILD @@ -6,7 +6,6 @@ cc_binary( "convert_time.cc", ], deps = [ - "//common:time_provider", "@fusion_engine_client", ], ) diff --git a/examples/convert_time/CMakeLists.txt b/examples/convert_time/CMakeLists.txt index b6987f9a9..6e534bd93 100644 --- a/examples/convert_time/CMakeLists.txt +++ b/examples/convert_time/CMakeLists.txt @@ -1,3 +1,2 @@ add_executable(convert_time convert_time.cc) target_link_libraries(convert_time PUBLIC fusion_engine_client) -target_link_libraries(convert_time PUBLIC time_provider) diff --git a/examples/convert_time/convert_time.cc b/examples/convert_time/convert_time.cc index 94e50e8b7..122549523 100644 --- a/examples/convert_time/convert_time.cc +++ b/examples/convert_time/convert_time.cc @@ -5,11 +5,10 @@ ******************************************************************************/ #include +#include -#include "../common/time_provider.h" - -using namespace point_one::fusion_engine::examples; using namespace point_one::fusion_engine::messages; +using namespace point_one::fusion_engine::utils; /******************************************************************************/ void PrintGPSTime(const Timestamp& gps_time) { diff --git a/examples/common/time_provider.cc b/src/point_one/fusion_engine/utils/time_provider.cc similarity index 95% rename from examples/common/time_provider.cc rename to src/point_one/fusion_engine/utils/time_provider.cc index e9cf6fdab..6f06cdc51 100644 --- a/examples/common/time_provider.cc +++ b/src/point_one/fusion_engine/utils/time_provider.cc @@ -1,12 +1,12 @@ /**************************************************************************/ /** - * @brief Example utility for converting between P1 and GPS time. + * @brief Helper utility for converting between P1 and GPS time. * @file ******************************************************************************/ -#include "time_provider.h" +#include "point_one/fusion_engine/utils/time_provider.h" -using namespace point_one::fusion_engine::examples; using namespace point_one::fusion_engine::messages; +using namespace point_one::fusion_engine::utils; /******************************************************************************/ void TimeProvider::Reset() { *this = TimeProvider(); } diff --git a/examples/common/time_provider.h b/src/point_one/fusion_engine/utils/time_provider.h similarity index 89% rename from examples/common/time_provider.h rename to src/point_one/fusion_engine/utils/time_provider.h index 20511fc00..e2272afc5 100644 --- a/examples/common/time_provider.h +++ b/src/point_one/fusion_engine/utils/time_provider.h @@ -1,5 +1,5 @@ /**************************************************************************/ /** - * @brief Example utility for converting between P1 and GPS time. + * @brief Helper utility for converting between P1 and GPS time. * @file ******************************************************************************/ @@ -9,7 +9,7 @@ namespace point_one { namespace fusion_engine { -namespace examples { +namespace utils { /** * @brief Utility for converting between P1 and GPS time. @@ -35,6 +35,6 @@ class TimeProvider { messages::Timestamp prev_gps_time_; }; -} // namespace examples +} // namespace utils } // namespace fusion_engine } // namespace point_one From 9cd789a3ac670dadd568fc2ffd52fdafe2156b97 Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Wed, 29 Apr 2026 11:40:35 -0400 Subject: [PATCH 10/52] Bazel formatting cleanup. --- BUILD | 30 +++++++++++++++--------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/BUILD b/BUILD index f70e30d71..b2a2c2001 100644 --- a/BUILD +++ b/BUILD @@ -105,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", @@ -150,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 = [ From 2888ce9b4cb4391891eab1331be1f5614232c497 Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Wed, 29 Apr 2026 11:42:53 -0400 Subject: [PATCH 11/52] Documented TimeProvivder functions. --- .../fusion_engine/utils/time_provider.h | 25 +++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/src/point_one/fusion_engine/utils/time_provider.h b/src/point_one/fusion_engine/utils/time_provider.h index e2272afc5..9d7007315 100644 --- a/src/point_one/fusion_engine/utils/time_provider.h +++ b/src/point_one/fusion_engine/utils/time_provider.h @@ -18,13 +18,38 @@ class 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: From b88be1b2e268ff2e8f84a0d320c1c0ea0d9162f0 Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Wed, 29 Apr 2026 12:02:10 -0400 Subject: [PATCH 12/52] Include iomanip in logging.h for convenience. --- src/point_one/fusion_engine/common/logging.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/point_one/fusion_engine/common/logging.h b/src/point_one/fusion_engine/common/logging.h index b7e682d20..b6454fbbd 100644 --- a/src/point_one/fusion_engine/common/logging.h +++ b/src/point_one/fusion_engine/common/logging.h @@ -17,6 +17,9 @@ #include "point_one/fusion_engine/common/portability.h" +// Included for convenience. +#include + // Use Google Logging Library (glog). #if P1_HAVE_GLOG && !P1_NO_LOGGING # include From 6f5fed69f31ac3582a1e1e17e6fbd6450521d77b Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Wed, 29 Apr 2026 12:02:59 -0400 Subject: [PATCH 13/52] Added debug prints to TimeProvider. --- BUILD | 1 + .../fusion_engine/utils/time_provider.cc | 103 ++++++++++++++++-- 2 files changed, 93 insertions(+), 11 deletions(-) diff --git a/BUILD b/BUILD index b2a2c2001..5bf0ce646 100644 --- a/BUILD +++ b/BUILD @@ -173,6 +173,7 @@ cc_library( "src/point_one/fusion_engine/utils/time_provider.h", ], deps = [ + ":common", ":core_headers", ], ) diff --git a/src/point_one/fusion_engine/utils/time_provider.cc b/src/point_one/fusion_engine/utils/time_provider.cc index 6f06cdc51..1dfaa1d1a 100644 --- a/src/point_one/fusion_engine/utils/time_provider.cc +++ b/src/point_one/fusion_engine/utils/time_provider.cc @@ -3,13 +3,61 @@ * @file ******************************************************************************/ + #define P1_VMODULE_NAME time_provider + #include "point_one/fusion_engine/utils/time_provider.h" +#include + using namespace point_one::fusion_engine::messages; using namespace point_one::fusion_engine::utils; +#include "point_one/fusion_engine/common/logging.h" + /******************************************************************************/ -void TimeProvider::Reset() { *this = TimeProvider(); } +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, @@ -25,48 +73,77 @@ void TimeProvider::HandleMessage(const MessageHeader& header, 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 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() || !current_p1_time_.IsValid() || - !current_gps_time_.IsValid()) { + 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_; double elapsed_gps_sec = current_gps_time_ - prev_gps_time_; double delta_p1_sec = p1_time - prev_p1_time_; double offset_sec = elapsed_gps_sec * delta_p1_sec / elapsed_p1_sec; int32_t int_sec = static_cast(offset_sec); - TimestampDelta delta_gps(int_sec, - static_cast((offset_sec - int_sec) * 1e9)); - return prev_gps_time_ + delta_gps; + TimestampDelta delta_gps( + int_sec, static_cast((offset_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 { - return p1_time + (current_gps_time_ - current_p1_time_); + gps_time = p1_time + (current_gps_time_ - current_p1_time_); } + + 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() || !current_p1_time_.IsValid() || - !current_gps_time_.IsValid()) { + 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_gps_sec = current_gps_time_ - prev_gps_time_; double elapsed_p1_sec = current_p1_time_ - prev_p1_time_; @@ -75,13 +152,17 @@ Timestamp TimeProvider::GPSToP1(const Timestamp& gps_time) const { int32_t int_sec = static_cast(offset_sec); TimestampDelta delta_p1(int_sec, static_cast((offset_sec - int_sec) * 1e9)); - return prev_p1_time_ + delta_p1; + 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 { - return gps_time + (current_p1_time_ - current_gps_time_); + p1_time = gps_time + (current_p1_time_ - current_gps_time_); } + + VLOG(2) << "Converted GPS " << GPSTimeFormat(gps_time) << " to P1 " + << P1TimeFormat(p1_time); + return p1_time; } From 0a86b112782cfb24ea560a0aea7f2bfe1ea4f2ac Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Wed, 29 Apr 2026 12:06:36 -0400 Subject: [PATCH 14/52] Print pose messages before processing in example. --- examples/convert_time/convert_time.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/convert_time/convert_time.cc b/examples/convert_time/convert_time.cc index 122549523..ab6903e13 100644 --- a/examples/convert_time/convert_time.cc +++ b/examples/convert_time/convert_time.cc @@ -55,9 +55,9 @@ int main(int argc, const char* argv[]) { PoseMessage pose1; pose1.p1_time = {1, 0}; pose1.gps_time = Timestamp::FromGPSTime(2416, 288018.0); - time_provider.HandleMessage(pose_header, &pose1); 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); @@ -81,9 +81,9 @@ int main(int argc, const char* argv[]) { PoseMessage pose2; pose2.p1_time = {1, 100'001'000}; pose2.gps_time = Timestamp::FromGPSTime(2416, 288018.1); - time_provider.HandleMessage(pose_header, &pose2); 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); From 1fc3a54f9e547e9a95c7af9b29c475358fe193aa Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Wed, 29 Apr 2026 13:09:04 -0400 Subject: [PATCH 15/52] Formatting cleanup. --- examples/convert_time/convert_time.cc | 3 +-- src/point_one/fusion_engine/utils/time_provider.cc | 6 +++--- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/examples/convert_time/convert_time.cc b/examples/convert_time/convert_time.cc index ab6903e13..8a8980ba3 100644 --- a/examples/convert_time/convert_time.cc +++ b/examples/convert_time/convert_time.cc @@ -16,8 +16,7 @@ void PrintGPSTime(const Timestamp& gps_time) { double tow_sec = NAN; if (gps_time.ToGPSWeekTOW(&week_number, &tow_sec)) { printf("Week %u, TOW %.9f sec\n", week_number, tow_sec); - } - else { + } else { printf("\n"); } } diff --git a/src/point_one/fusion_engine/utils/time_provider.cc b/src/point_one/fusion_engine/utils/time_provider.cc index 1dfaa1d1a..577b8e1a1 100644 --- a/src/point_one/fusion_engine/utils/time_provider.cc +++ b/src/point_one/fusion_engine/utils/time_provider.cc @@ -3,7 +3,7 @@ * @file ******************************************************************************/ - #define P1_VMODULE_NAME time_provider +#define P1_VMODULE_NAME time_provider #include "point_one/fusion_engine/utils/time_provider.h" @@ -93,7 +93,7 @@ void TimeProvider::HandleMessage(const MessageHeader& header, /******************************************************************************/ Timestamp TimeProvider::P1ToGPS(const Timestamp& p1_time) const { - if (!p1_time.IsValid()){ + 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()) { @@ -131,7 +131,7 @@ Timestamp TimeProvider::P1ToGPS(const Timestamp& p1_time) const { /******************************************************************************/ Timestamp TimeProvider::GPSToP1(const Timestamp& gps_time) const { - if (!gps_time.IsValid()){ + 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()) { From 2d4d10219f8302f5bd4ababbd478446e189e563b Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Wed, 29 Apr 2026 13:12:16 -0400 Subject: [PATCH 16/52] Removed digit separators for C++ compatibility. --- examples/convert_time/convert_time.cc | 8 +++---- .../fusion_engine/messages/timestamp.h | 24 +++++++++---------- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/examples/convert_time/convert_time.cc b/examples/convert_time/convert_time.cc index 8a8980ba3..7ab980331 100644 --- a/examples/convert_time/convert_time.cc +++ b/examples/convert_time/convert_time.cc @@ -43,7 +43,7 @@ int main(int argc, const char* argv[]) { // not gotten any pose messages yet, so we do not know the P1/GPS time // relationship. IMUOutput imu1; - imu1.p1_time = {0, 999'000'000}; + imu1.p1_time = {0, 999000000}; printf("IMU measurement 1:\n"); PrintIMUMeasurement(time_provider, imu1); @@ -63,7 +63,7 @@ int main(int argc, const char* argv[]) { // Step 3: Convert the next IMU measurement's P1 time to GPS time. IMUOutput imu2; - imu2.p1_time = {1, 100'000'500}; + imu2.p1_time = {1, 100000500}; printf("IMU measurement 2:\n"); PrintIMUMeasurement(time_provider, imu2); @@ -78,7 +78,7 @@ int main(int argc, const char* argv[]) { // 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, 100'001'000}; + 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); @@ -89,7 +89,7 @@ int main(int argc, const char* argv[]) { // Step 5: Convert a 3rd IMU measurement with interpolation applied. IMUOutput imu3; - imu3.p1_time = {1, 100'001'500}; + imu3.p1_time = {1, 100001500}; printf("IMU measurement 3:\n"); PrintIMUMeasurement(time_provider, imu3); diff --git a/src/point_one/fusion_engine/messages/timestamp.h b/src/point_one/fusion_engine/messages/timestamp.h index 957ef4a1b..748148b70 100644 --- a/src/point_one/fusion_engine/messages/timestamp.h +++ b/src/point_one/fusion_engine/messages/timestamp.h @@ -202,22 +202,22 @@ struct TimestampDelta { void Normalize() { // Convert |fraction_ns| to <1 second. This must be done before sign // alignment. - if (fraction_ns >= 1'000'000'000) { - seconds += fraction_ns / 1'000'000'000; - fraction_ns %= 1'000'000'000; - } else if (fraction_ns <= -1'000'000'000) { - seconds += fraction_ns / 1'000'000'000; - fraction_ns %= 1'000'000'000; + 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 -= 1'000'000'000; + fraction_ns -= 1000000000; } else if (fraction_ns < 0 && seconds > 0) { seconds -= 1; - fraction_ns += 1'000'000'000; + fraction_ns += 1000000000; } } @@ -356,10 +356,10 @@ inline Timestamp operator+(Timestamp lhs, const TimestampDelta& rhs) { int64_t ns = static_cast(lhs.fraction_ns) + rhs.fraction_ns; if (ns < 0) { sec -= 1; - ns += 1'000'000'000; - } else if (ns >= 1'000'000'000) { - sec += ns / 1'000'000'000; - ns %= 1'000'000'000; + ns += 1000000000; + } else if (ns >= 1000000000) { + sec += ns / 1000000000; + ns %= 1000000000; } if (sec < 0 || sec > static_cast(Timestamp::INVALID - 1)) { From d8b3a07b6b6bb24bfa0c9431459a704ff9137e21 Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Wed, 29 Apr 2026 13:20:58 -0400 Subject: [PATCH 17/52] Added default and 2-arg Timestamp constructors. --- src/point_one/fusion_engine/messages/timestamp.h | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/point_one/fusion_engine/messages/timestamp.h b/src/point_one/fusion_engine/messages/timestamp.h index 748148b70..4d19d52f3 100644 --- a/src/point_one/fusion_engine/messages/timestamp.h +++ b/src/point_one/fusion_engine/messages/timestamp.h @@ -34,6 +34,17 @@ struct P1_ALIGNAS(4) Timestamp { /** 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. * From dfa49e518ebf16c5461b5b2de255e4c645281327 Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Wed, 29 Apr 2026 13:43:20 -0400 Subject: [PATCH 18/52] Added Timestamp.is_valid() to Python. --- python/fusion_engine_client/messages/timestamp.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/python/fusion_engine_client/messages/timestamp.py b/python/fusion_engine_client/messages/timestamp.py index 931de507c..a793a48a6 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) @@ -142,7 +145,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 From ab070ca290bc19fda3ea7cbfe8fc3b2eab82a52e Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Wed, 29 Apr 2026 13:43:31 -0400 Subject: [PATCH 19/52] Added timedelta operator support to Timestamp. --- .../messages/timestamp.py | 30 +++++++++++++++---- 1 file changed, 25 insertions(+), 5 deletions(-) diff --git a/python/fusion_engine_client/messages/timestamp.py b/python/fusion_engine_client/messages/timestamp.py index a793a48a6..9713ddf3a 100644 --- a/python/fusion_engine_client/messages/timestamp.py +++ b/python/fusion_engine_client/messages/timestamp.py @@ -113,17 +113,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): From 3653c6512866f79ad1e7db88217822e1fc53ab38 Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Wed, 29 Apr 2026 15:50:07 -0400 Subject: [PATCH 20/52] Added implict TimestampDelta(double) construction. --- src/point_one/fusion_engine/messages/timestamp.h | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/point_one/fusion_engine/messages/timestamp.h b/src/point_one/fusion_engine/messages/timestamp.h index 4d19d52f3..0f1fce94d 100644 --- a/src/point_one/fusion_engine/messages/timestamp.h +++ b/src/point_one/fusion_engine/messages/timestamp.h @@ -175,6 +175,16 @@ struct TimestampDelta { } } + /** + * @brief Construct a delta from a floating point second value. + * + * @param sec The second value. + */ + TimestampDelta(double sec) { + seconds = static_cast(sec); + fraction_ns = static_cast(std::lround((sec - seconds) * 1e9)); + } + /** * @brief Check if this delta is valid. * From b820de8dfa6b3f0df3968bf051ac6b2f26e306fe Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Wed, 29 Apr 2026 15:04:56 -0400 Subject: [PATCH 21/52] Consistency cleanup. --- .../fusion_engine/utils/time_provider.cc | 29 ++++++++++--------- 1 file changed, 16 insertions(+), 13 deletions(-) diff --git a/src/point_one/fusion_engine/utils/time_provider.cc b/src/point_one/fusion_engine/utils/time_provider.cc index 577b8e1a1..a9051254d 100644 --- a/src/point_one/fusion_engine/utils/time_provider.cc +++ b/src/point_one/fusion_engine/utils/time_provider.cc @@ -63,8 +63,8 @@ void TimeProvider::Reset() { void TimeProvider::HandleMessage(const MessageHeader& header, const void* payload) { if (header.message_type == MessageType::POSE) { - // Store the current and previous P1/GPS times, and use them to to convert - // to/from P1 or GPS time by interpolating. + // 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. @@ -75,7 +75,8 @@ void TimeProvider::HandleMessage(const MessageHeader& header, current_gps_time_ = message.gps_time; if (VLOG_IS_ON(1)) { - VLOG(1) << "Received time update at:"; + 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_ && @@ -110,10 +111,10 @@ Timestamp TimeProvider::P1ToGPS(const Timestamp& p1_time) const { double elapsed_p1_sec = current_p1_time_ - prev_p1_time_; double elapsed_gps_sec = current_gps_time_ - prev_gps_time_; double delta_p1_sec = p1_time - prev_p1_time_; - double offset_sec = elapsed_gps_sec * delta_p1_sec / elapsed_p1_sec; - int32_t int_sec = static_cast(offset_sec); + double delta_gps_sec = elapsed_gps_sec * delta_p1_sec / elapsed_p1_sec; + int32_t int_sec = static_cast(delta_gps_sec); TimestampDelta delta_gps( - int_sec, static_cast((offset_sec - int_sec) * 1e9)); + 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 @@ -121,7 +122,8 @@ Timestamp TimeProvider::P1ToGPS(const Timestamp& p1_time) const { // time, but for most purposes it will be fine as long as current_*_time_ is // recent. else { - gps_time = p1_time + (current_gps_time_ - current_p1_time_); + double offset_sec = (current_gps_time_ - current_p1_time_); + gps_time = p1_time + offset_sec; } VLOG(2) << "Converted P1 " << P1TimeFormat(p1_time) << " to GPS " @@ -145,13 +147,13 @@ Timestamp TimeProvider::GPSToP1(const Timestamp& gps_time) const { // most accurate result. Timestamp p1_time; if (prev_gps_time_.IsValid() && prev_p1_time_.IsValid()) { - double elapsed_gps_sec = current_gps_time_ - prev_gps_time_; double elapsed_p1_sec = current_p1_time_ - prev_p1_time_; + double elapsed_gps_sec = current_gps_time_ - prev_gps_time_; double delta_gps_sec = gps_time - prev_gps_time_; - double offset_sec = elapsed_p1_sec * delta_gps_sec / elapsed_gps_sec; - int32_t int_sec = static_cast(offset_sec); - TimestampDelta delta_p1(int_sec, - static_cast((offset_sec - int_sec) * 1e9)); + double delta_p1_sec = elapsed_p1_sec * delta_gps_sec / elapsed_gps_sec; + int32_t int_sec = static_cast(delta_p1_sec); + TimestampDelta 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 @@ -159,7 +161,8 @@ Timestamp TimeProvider::GPSToP1(const Timestamp& gps_time) const { // time, but for most purposes it will be fine as long as current_*_time_ is // recent. else { - p1_time = gps_time + (current_p1_time_ - current_gps_time_); + double offset_sec = (current_p1_time_ - current_gps_time_); + p1_time = gps_time + offset_sec; } VLOG(2) << "Converted GPS " << GPSTimeFormat(gps_time) << " to P1 " From 818a3ae25be1eb69b336956fe0387898e91e0805 Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Wed, 29 Apr 2026 16:07:49 -0400 Subject: [PATCH 22/52] Removed implicit operator double() to avoid conflict with TimestampDelta(double). --- .../fusion_engine/messages/timestamp.h | 15 -------------- .../fusion_engine/utils/time_provider.cc | 20 +++++++++---------- 2 files changed, 10 insertions(+), 25 deletions(-) diff --git a/src/point_one/fusion_engine/messages/timestamp.h b/src/point_one/fusion_engine/messages/timestamp.h index 0f1fce94d..b50e540c2 100644 --- a/src/point_one/fusion_engine/messages/timestamp.h +++ b/src/point_one/fusion_engine/messages/timestamp.h @@ -94,13 +94,6 @@ struct P1_ALIGNAS(4) Timestamp { */ operator bool() const { return IsValid(); } - /** - * @brief Get the timestamp value in seconds. - * - * @return The timestamp value (in seconds), or `NAN` if invalid. - */ - operator double() const { return ToSeconds(); } - /** * @brief Construct a @ref Timestamp object from a GPS week number and time of * week. @@ -249,14 +242,6 @@ struct TimestampDelta { */ operator bool() const { return IsValid(); } - /** - * @brief Convert this delta to a floating-point number of seconds. - * - * @return The delta expressed as seconds, or `NAN` if the delta is - * invalid. - */ - operator double() const { return ToSeconds(); } - /** * @brief Add another @ref TimestampDelta to this one in place. * diff --git a/src/point_one/fusion_engine/utils/time_provider.cc b/src/point_one/fusion_engine/utils/time_provider.cc index a9051254d..e266a8cba 100644 --- a/src/point_one/fusion_engine/utils/time_provider.cc +++ b/src/point_one/fusion_engine/utils/time_provider.cc @@ -108,9 +108,9 @@ Timestamp TimeProvider::P1ToGPS(const Timestamp& p1_time) const { // 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_; - double elapsed_gps_sec = current_gps_time_ - prev_gps_time_; - double delta_p1_sec = p1_time - prev_p1_time_; + 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); TimestampDelta delta_gps( @@ -122,8 +122,8 @@ Timestamp TimeProvider::P1ToGPS(const Timestamp& p1_time) const { // 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_); - gps_time = p1_time + offset_sec; + double offset_sec = (current_gps_time_ - current_p1_time_).ToSeconds(); + gps_time = p1_time + TimestampDelta(offset_sec); } VLOG(2) << "Converted P1 " << P1TimeFormat(p1_time) << " to GPS " @@ -147,9 +147,9 @@ Timestamp TimeProvider::GPSToP1(const Timestamp& gps_time) const { // 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_; - double elapsed_gps_sec = current_gps_time_ - prev_gps_time_; - double delta_gps_sec = gps_time - prev_gps_time_; + 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); TimestampDelta delta_p1( @@ -161,8 +161,8 @@ Timestamp TimeProvider::GPSToP1(const Timestamp& gps_time) const { // 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_); - p1_time = gps_time + offset_sec; + double offset_sec = (current_p1_time_ - current_gps_time_).ToSeconds(); + p1_time = gps_time + TimestampDelta(offset_sec); } VLOG(2) << "Converted GPS " << GPSTimeFormat(gps_time) << " to P1 " From 59273df61e31126874b50b6890a9a9c68254502b Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Wed, 29 Apr 2026 15:05:19 -0400 Subject: [PATCH 23/52] Added Python TimeProvider class. --- .../utils/time_provider.py | 121 ++++++++++++++++++ 1 file changed, 121 insertions(+) create mode 100644 python/fusion_engine_client/utils/time_provider.py 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..c8ded8f85 --- /dev/null +++ b/python/fusion_engine_client/utils/time_provider.py @@ -0,0 +1,121 @@ +from typing import Optional + +from datetime import timedelta + +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 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): + # 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) -> Timestamp: + """! + @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. + """ + if not p1_time: + _logger.trace('Cannot convert invalid P1 time to GPS time.') + 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.') + 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()) + return gps_time + + def gps_to_p1(self, gps_time: Timestamp) -> Timestamp: + """! + @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. + """ + if not gps_time: + _logger.trace('Cannot convert invalid GPS time to P1 time.') + return Timestamp() + elif 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 + From 7644cae35cf318b5fd4fc4bef67cf8c0794d51bb Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Wed, 29 Apr 2026 15:44:18 -0400 Subject: [PATCH 24/52] Added Timestamp.from_datetime() function. --- python/fusion_engine_client/messages/timestamp.py | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/python/fusion_engine_client/messages/timestamp.py b/python/fusion_engine_client/messages/timestamp.py index 9713ddf3a..f6de74cbf 100644 --- a/python/fusion_engine_client/messages/timestamp.py +++ b/python/fusion_engine_client/messages/timestamp.py @@ -75,12 +75,19 @@ 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()) + def pack(self, buffer: bytes = None, offset: int = 0, return_buffer: bool = False) -> (bytes, int): if math.isnan(self.seconds): int_part = Timestamp._INVALID From 09f828f2d84f301401fcae1b95fcb38dbb2c2231 Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Wed, 29 Apr 2026 15:44:28 -0400 Subject: [PATCH 25/52] Added datetime/gpstime support to TimeProvider. --- .../utils/time_provider.py | 36 ++++++++++++++----- 1 file changed, 27 insertions(+), 9 deletions(-) diff --git a/python/fusion_engine_client/utils/time_provider.py b/python/fusion_engine_client/utils/time_provider.py index c8ded8f85..76ff94fab 100644 --- a/python/fusion_engine_client/utils/time_provider.py +++ b/python/fusion_engine_client/utils/time_provider.py @@ -1,6 +1,8 @@ -from typing import Optional +from typing import Optional, Union -from datetime import timedelta +from datetime import datetime, timezone, timedelta + +from gpstime import gpstime from ..messages import MessageHeader, MessagePayload, PoseMessage, Timestamp from ..utils import trace as logging @@ -49,21 +51,30 @@ def handle_message(self, message: MessagePayload, header: Optional[MessageHeader P1/GPS: {scale_sps_str} """) - def p1_to_gps(self, p1_time: Timestamp) -> Timestamp: + 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.') - return Timestamp() + 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.') - return Timestamp() + 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. @@ -82,20 +93,27 @@ def p1_to_gps(self, p1_time: Timestamp) -> Timestamp: if _logger.isEnabledFor(logging.TRACE): _logger.trace('Converted P1 %s to GPS %s.', p1_time.to_p1_str(), gps_time.to_gps_str()) - return gps_time - def gps_to_p1(self, gps_time: Timestamp) -> Timestamp: + 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 to convert. + @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 not self._current_gps_time or not self._current_p1_time: + 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() From 8e018eaa05e751e284f1b5585270d8c451c98ae4 Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Wed, 29 Apr 2026 15:58:22 -0400 Subject: [PATCH 26/52] Added Python TimeProvider unit tests. --- python/tests/test_time_provider.py | 163 +++++++++++++++++++++++++++++ 1 file changed, 163 insertions(+) create mode 100644 python/tests/test_time_provider.py diff --git a/python/tests/test_time_provider.py b/python/tests/test_time_provider.py new file mode 100644 index 000000000..7957c4dfc --- /dev/null +++ b/python/tests/test_time_provider.py @@ -0,0 +1,163 @@ +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) + + +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) From 9af70835a721e96f6fbea2b3ae264778d74c1644 Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Thu, 30 Apr 2026 08:20:48 -0400 Subject: [PATCH 27/52] Renamed TimestampDelta -> TimeDelta. --- .../fusion_engine/messages/timestamp.h | 52 +++++++++---------- .../fusion_engine/utils/time_provider.cc | 12 ++--- 2 files changed, 31 insertions(+), 33 deletions(-) diff --git a/src/point_one/fusion_engine/messages/timestamp.h b/src/point_one/fusion_engine/messages/timestamp.h index b50e540c2..3c753ccde 100644 --- a/src/point_one/fusion_engine/messages/timestamp.h +++ b/src/point_one/fusion_engine/messages/timestamp.h @@ -118,8 +118,8 @@ struct P1_ALIGNAS(4) Timestamp { * Timestamp values. * * Unlike @ref Timestamp, which represents an absolute point in time using - * unsigned fields, `TimestampDelta` uses signed integers to express durations - * that may be negative (i.e., in the past relative to a reference time). + * 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 @@ -130,7 +130,7 @@ struct P1_ALIGNAS(4) Timestamp { * Both fields are set to @ref INVALID (`INT32_MIN`) to indicate an invalid or * unknown delta. */ -struct TimestampDelta { +struct TimeDelta { static constexpr int32_t INVALID = INT32_MIN; /** @@ -149,20 +149,20 @@ struct TimestampDelta { int32_t fraction_ns = INVALID; /** @brief Construct an invalid delta. */ - TimestampDelta() = default; + 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, `TimestampDelta(0, 1500000000)` produces + * @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. */ - TimestampDelta(int32_t sec, int32_t ns) : seconds(sec), fraction_ns(ns) { + TimeDelta(int32_t sec, int32_t ns) : seconds(sec), fraction_ns(ns) { if (seconds != INVALID && fraction_ns != INVALID) { Normalize(); } @@ -173,7 +173,7 @@ struct TimestampDelta { * * @param sec The second value. */ - TimestampDelta(double sec) { + TimeDelta(double sec) { seconds = static_cast(sec); fraction_ns = static_cast(std::lround((sec - seconds) * 1e9)); } @@ -243,7 +243,7 @@ struct TimestampDelta { operator bool() const { return IsValid(); } /** - * @brief Add another @ref TimestampDelta to this one in place. + * @brief Add another @ref TimeDelta to this one in place. * * If either operand is invalid, the result is set to @ref INVALID. * @@ -251,7 +251,7 @@ struct TimestampDelta { * * @return A reference to this delta after addition. */ - TimestampDelta& operator+=(const TimestampDelta& rhs) { + TimeDelta& operator+=(const TimeDelta& rhs) { if (IsValid()) { if (rhs.IsValid()) { seconds += rhs.seconds; @@ -265,7 +265,7 @@ struct TimestampDelta { } /** - * @brief Subtract another @ref TimestampDelta from this one in place. + * @brief Subtract another @ref TimeDelta from this one in place. * * If either operand is invalid, the result is set to @ref INVALID. * @@ -273,7 +273,7 @@ struct TimestampDelta { * * @return A reference to this delta after subtraction. */ - TimestampDelta& operator-=(const TimestampDelta& rhs) { + TimeDelta& operator-=(const TimeDelta& rhs) { if (IsValid()) { if (rhs.IsValid()) { seconds -= rhs.seconds; @@ -287,7 +287,7 @@ struct TimestampDelta { } /** - * @brief Add two @ref TimestampDelta values. + * @brief Add two @ref TimeDelta values. * * If either operand is invalid, the result is @ref INVALID. * @@ -296,14 +296,13 @@ struct TimestampDelta { * * @return The sum of the two deltas. */ - friend TimestampDelta operator+(TimestampDelta lhs, - const TimestampDelta& rhs) { + friend TimeDelta operator+(TimeDelta lhs, const TimeDelta& rhs) { lhs += rhs; return lhs; } /** - * @brief Subtract one @ref TimestampDelta from another. + * @brief Subtract one @ref TimeDelta from another. * * If either operand is invalid, the result is @ref INVALID. * @@ -312,8 +311,7 @@ struct TimestampDelta { * * @return The difference of the two deltas. */ - friend TimestampDelta operator-(TimestampDelta lhs, - const TimestampDelta& rhs) { + friend TimeDelta operator-(TimeDelta lhs, const TimeDelta& rhs) { lhs -= rhs; return lhs; } @@ -321,20 +319,20 @@ struct TimestampDelta { /** * @brief Compute the difference between two @ref Timestamp values as a - * @ref TimestampDelta. + * @ref TimeDelta. * * @param lhs The timestamp to subtract from. * @param rhs The timestamp to subtract. * - * @return A @ref TimestampDelta representing `lhs - rhs`, or an invalid + * @return A @ref TimeDelta representing `lhs - rhs`, or an invalid * delta if either operand is invalid. */ -inline TimestampDelta operator-(const Timestamp& lhs, const Timestamp& rhs) { +inline TimeDelta operator-(const Timestamp& lhs, const Timestamp& rhs) { if (!lhs.IsValid() || !rhs.IsValid()) { - return TimestampDelta(); + return TimeDelta(); } - TimestampDelta result; + TimeDelta result; result.seconds = static_cast(lhs.seconds) - static_cast(rhs.seconds); result.fraction_ns = static_cast(lhs.fraction_ns) - @@ -344,7 +342,7 @@ inline TimestampDelta operator-(const Timestamp& lhs, const Timestamp& rhs) { } /** - * @brief Offset a @ref Timestamp forward by a @ref TimestampDelta. + * @brief Offset a @ref Timestamp forward by a @ref TimeDelta. * * @param lhs The base timestamp. * @param rhs The delta to add. @@ -353,7 +351,7 @@ inline TimestampDelta operator-(const Timestamp& lhs, const Timestamp& rhs) { * timestamp if either operand is invalid or the result falls outside * the representable range of @ref Timestamp. */ -inline Timestamp operator+(Timestamp lhs, const TimestampDelta& rhs) { +inline Timestamp operator+(Timestamp lhs, const TimeDelta& rhs) { if (!lhs.IsValid() || !rhs.IsValid()) { return Timestamp(); } @@ -376,7 +374,7 @@ inline Timestamp operator+(Timestamp lhs, const TimestampDelta& rhs) { } /** - * @brief Offset a @ref Timestamp backwards by a @ref TimestampDelta. + * @brief Offset a @ref Timestamp backwards by a @ref TimeDelta. * * @param lhs The base timestamp. * @param rhs The delta to subtract. @@ -385,12 +383,12 @@ inline Timestamp operator+(Timestamp lhs, const TimestampDelta& rhs) { * timestamp if either operand is invalid or the result falls outside * the representable range of @ref Timestamp. */ -inline Timestamp operator-(const Timestamp& lhs, const TimestampDelta& rhs) { +inline Timestamp operator-(const Timestamp& lhs, const TimeDelta& rhs) { if (!rhs.IsValid()) { return Timestamp(); } - TimestampDelta negated{-rhs.seconds, -rhs.fraction_ns}; + TimeDelta negated{-rhs.seconds, -rhs.fraction_ns}; negated.Normalize(); return lhs + negated; } diff --git a/src/point_one/fusion_engine/utils/time_provider.cc b/src/point_one/fusion_engine/utils/time_provider.cc index e266a8cba..85e604cf6 100644 --- a/src/point_one/fusion_engine/utils/time_provider.cc +++ b/src/point_one/fusion_engine/utils/time_provider.cc @@ -113,8 +113,8 @@ Timestamp TimeProvider::P1ToGPS(const Timestamp& p1_time) const { 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); - TimestampDelta delta_gps( - int_sec, static_cast((delta_gps_sec - int_sec) * 1e9)); + 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 @@ -123,7 +123,7 @@ Timestamp TimeProvider::P1ToGPS(const Timestamp& p1_time) const { // recent. else { double offset_sec = (current_gps_time_ - current_p1_time_).ToSeconds(); - gps_time = p1_time + TimestampDelta(offset_sec); + gps_time = p1_time + TimeDelta(offset_sec); } VLOG(2) << "Converted P1 " << P1TimeFormat(p1_time) << " to GPS " @@ -152,8 +152,8 @@ Timestamp TimeProvider::GPSToP1(const Timestamp& gps_time) const { 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); - TimestampDelta delta_p1( - int_sec, static_cast((delta_p1_sec - int_sec) * 1e9)); + 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 @@ -162,7 +162,7 @@ Timestamp TimeProvider::GPSToP1(const Timestamp& gps_time) const { // recent. else { double offset_sec = (current_p1_time_ - current_gps_time_).ToSeconds(); - p1_time = gps_time + TimestampDelta(offset_sec); + p1_time = gps_time + TimeDelta(offset_sec); } VLOG(2) << "Converted GPS " << GPSTimeFormat(gps_time) << " to P1 " From ffc106bae40c686ff86f4fb24e2ae074ab19fc4a Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Thu, 30 Apr 2026 08:52:10 -0400 Subject: [PATCH 28/52] Added reset function. --- python/fusion_engine_client/utils/time_provider.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/python/fusion_engine_client/utils/time_provider.py b/python/fusion_engine_client/utils/time_provider.py index 76ff94fab..7e57e6fc2 100644 --- a/python/fusion_engine_client/utils/time_provider.py +++ b/python/fusion_engine_client/utils/time_provider.py @@ -20,6 +20,12 @@ def __init__(self): 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. From cf9f44f95f7669517c8ddfb14eb68a477c820896 Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Thu, 30 Apr 2026 08:52:21 -0400 Subject: [PATCH 29/52] Formatting cleanup. --- src/point_one/fusion_engine/utils/time_provider.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/point_one/fusion_engine/utils/time_provider.cc b/src/point_one/fusion_engine/utils/time_provider.cc index 85e604cf6..a69f4310a 100644 --- a/src/point_one/fusion_engine/utils/time_provider.cc +++ b/src/point_one/fusion_engine/utils/time_provider.cc @@ -9,11 +9,11 @@ #include +#include "point_one/fusion_engine/common/logging.h" + using namespace point_one::fusion_engine::messages; using namespace point_one::fusion_engine::utils; -#include "point_one/fusion_engine/common/logging.h" - /******************************************************************************/ class P1TimeFormat { public: From 8304f944c26982109cb3eae5a4e16e8bce231dd1 Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Thu, 30 Apr 2026 08:52:34 -0400 Subject: [PATCH 30/52] Sanity check for backwards/duplicate timestamps. --- .../utils/time_provider.py | 16 ++++ python/tests/test_time_provider.py | 73 +++++++++++++++++++ .../fusion_engine/utils/time_provider.cc | 27 ++++++- 3 files changed, 115 insertions(+), 1 deletion(-) diff --git a/python/fusion_engine_client/utils/time_provider.py b/python/fusion_engine_client/utils/time_provider.py index 7e57e6fc2..45260d312 100644 --- a/python/fusion_engine_client/utils/time_provider.py +++ b/python/fusion_engine_client/utils/time_provider.py @@ -34,6 +34,22 @@ def handle_message(self, message: MessagePayload, header: Optional[MessageHeader @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). # diff --git a/python/tests/test_time_provider.py b/python/tests/test_time_provider.py index 7957c4dfc..a60372f3a 100644 --- a/python/tests/test_time_provider.py +++ b/python/tests/test_time_provider.py @@ -41,6 +41,79 @@ def test_advances_prev_times(self): 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): diff --git a/src/point_one/fusion_engine/utils/time_provider.cc b/src/point_one/fusion_engine/utils/time_provider.cc index a69f4310a..dcd7d7e46 100644 --- a/src/point_one/fusion_engine/utils/time_provider.cc +++ b/src/point_one/fusion_engine/utils/time_provider.cc @@ -63,12 +63,37 @@ void TimeProvider::Reset() { 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. - auto& message = *reinterpret_cast(payload); prev_p1_time_ = current_p1_time_; prev_gps_time_ = current_gps_time_; current_p1_time_ = message.p1_time; From 78956e686d17e10a60fc90218cb94752b6627d8c Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Thu, 30 Apr 2026 10:26:40 -0400 Subject: [PATCH 31/52] Minor include cleanup. --- examples/convert_time/convert_time.cc | 3 +++ src/point_one/fusion_engine/utils/time_provider.cc | 2 ++ 2 files changed, 5 insertions(+) diff --git a/examples/convert_time/convert_time.cc b/examples/convert_time/convert_time.cc index 7ab980331..c207351da 100644 --- a/examples/convert_time/convert_time.cc +++ b/examples/convert_time/convert_time.cc @@ -4,6 +4,9 @@ * @file ******************************************************************************/ +#include +#include + #include #include diff --git a/src/point_one/fusion_engine/utils/time_provider.cc b/src/point_one/fusion_engine/utils/time_provider.cc index dcd7d7e46..f0e91abad 100644 --- a/src/point_one/fusion_engine/utils/time_provider.cc +++ b/src/point_one/fusion_engine/utils/time_provider.cc @@ -7,6 +7,8 @@ #include "point_one/fusion_engine/utils/time_provider.h" +#include +#include #include #include "point_one/fusion_engine/common/logging.h" From 115958c6057606e459a1bed0ef243ac9a274e7be Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Thu, 30 Apr 2026 10:26:56 -0400 Subject: [PATCH 32/52] Sanity check week/TOW and second values. --- .../fusion_engine/messages/timestamp.h | 22 ++++++++++++++----- 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/src/point_one/fusion_engine/messages/timestamp.h b/src/point_one/fusion_engine/messages/timestamp.h index 3c753ccde..0cd355c5c 100644 --- a/src/point_one/fusion_engine/messages/timestamp.h +++ b/src/point_one/fusion_engine/messages/timestamp.h @@ -105,10 +105,16 @@ struct P1_ALIGNAS(4) Timestamp { */ static Timestamp FromGPSTime(uint16_t week_number, double tow_sec) { Timestamp result; - 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 (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; } }; @@ -174,8 +180,12 @@ struct TimeDelta { * @param sec The second value. */ TimeDelta(double sec) { - seconds = static_cast(sec); - fraction_ns = static_cast(std::lround((sec - seconds) * 1e9)); + 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(); + } } /** From e2019df01131f29aeb963e82d956eb6f23c9b497 Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Thu, 30 Apr 2026 10:29:15 -0400 Subject: [PATCH 33/52] Revert "Include iomanip in logging.h for convenience." --- src/point_one/fusion_engine/common/logging.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/point_one/fusion_engine/common/logging.h b/src/point_one/fusion_engine/common/logging.h index b6454fbbd..b7e682d20 100644 --- a/src/point_one/fusion_engine/common/logging.h +++ b/src/point_one/fusion_engine/common/logging.h @@ -17,9 +17,6 @@ #include "point_one/fusion_engine/common/portability.h" -// Included for convenience. -#include - // Use Google Logging Library (glog). #if P1_HAVE_GLOG && !P1_NO_LOGGING # include From cddbb7e2a3230dcfef740b5d3e021910f8b9bf3c Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Thu, 30 Apr 2026 10:31:44 -0400 Subject: [PATCH 34/52] Added Timestamp unit tests. --- python/tests/test_timestamp.py | 286 +++++++++++++++++++++++++++++++++ 1 file changed, 286 insertions(+) create mode 100644 python/tests/test_timestamp.py diff --git a/python/tests/test_timestamp.py b/python/tests/test_timestamp.py new file mode 100644 index 000000000..82ccba690 --- /dev/null +++ b/python/tests/test_timestamp.py @@ -0,0 +1,286 @@ +from datetime import datetime, timedelta, timezone +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 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(' Date: Thu, 30 Apr 2026 10:42:11 -0400 Subject: [PATCH 35/52] Added Timestamp.from_gps_week_tow() function. --- .../messages/timestamp.py | 7 ++++ python/tests/test_timestamp.py | 36 +++++++++++++++++++ 2 files changed, 43 insertions(+) diff --git a/python/fusion_engine_client/messages/timestamp.py b/python/fusion_engine_client/messages/timestamp.py index f6de74cbf..4ec1b2e1c 100644 --- a/python/fusion_engine_client/messages/timestamp.py +++ b/python/fusion_engine_client/messages/timestamp.py @@ -88,6 +88,13 @@ def from_datetime(cls, time: Union[datetime, gpstime]) -> 'Timestamp': 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 diff --git a/python/tests/test_timestamp.py b/python/tests/test_timestamp.py index 82ccba690..7c7481661 100644 --- a/python/tests/test_timestamp.py +++ b/python/tests/test_timestamp.py @@ -128,6 +128,42 @@ def test_from_datetime_is_gps(self): 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 From 65294163316ace97986a53d3df760ae05ec247e4 Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Thu, 30 Apr 2026 11:05:34 -0400 Subject: [PATCH 36/52] Added gtest to Bazel workspace. --- WORKSPACE | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/WORKSPACE b/WORKSPACE index b151a07b2..25fb4b7de 100644 --- a/WORKSPACE +++ b/WORKSPACE @@ -1 +1,10 @@ workspace(name = "p1_fusion_engine_client") + +load("@bazel_tools//tools/build_defs/repo:http.bzl", "http_archive") + +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"], +) From c8a2a3447fab4a73753a33c95f9c19d7fbd651e7 Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Thu, 30 Apr 2026 11:05:51 -0400 Subject: [PATCH 37/52] Added Timestamp C++ unit tests. --- BUILD | 14 + .../fusion_engine/messages/timestamp_test.cc | 348 ++++++++++++++++++ 2 files changed, 362 insertions(+) create mode 100644 src/point_one/fusion_engine/messages/timestamp_test.cc diff --git a/BUILD b/BUILD index 5bf0ce646..ad4b89c9a 100644 --- a/BUILD +++ b/BUILD @@ -177,3 +177,17 @@ cc_library( ":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", + ], +) 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); +} From 854fd28810fd11e806a7e6664e20facd8416fd7e Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Thu, 30 Apr 2026 11:27:01 -0400 Subject: [PATCH 38/52] Added TimeProvider C++ unit tests. --- BUILD | 11 + .../fusion_engine/utils/time_provider_test.cc | 283 ++++++++++++++++++ 2 files changed, 294 insertions(+) create mode 100644 src/point_one/fusion_engine/utils/time_provider_test.cc diff --git a/BUILD b/BUILD index ad4b89c9a..460df88e6 100644 --- a/BUILD +++ b/BUILD @@ -191,3 +191,14 @@ cc_test( "@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/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); +} From 16ca5517ce1483fe1606b592d2dde72bc19c2cdb Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Thu, 30 Apr 2026 11:37:21 -0400 Subject: [PATCH 39/52] Use //:all in CI builds. --- .github/workflows/release_build.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/release_build.yml b/.github/workflows/release_build.yml index 505a1f1cc..4fa5520b1 100644 --- a/.github/workflows/release_build.yml +++ b/.github/workflows/release_build.yml @@ -116,12 +116,12 @@ jobs: - name: Build Library run: | - bazel build -c opt //:* + bazel build -c opt //:all - name: Build Examples run: | cd examples && - bazel build -c opt //:* + bazel build -c opt //:all - name: Install Test Files run: | From 035bf5859f493211badd61db62c7f82348d8e719 Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Thu, 30 Apr 2026 11:37:30 -0400 Subject: [PATCH 40/52] Run unit tests in Bazel CI build. --- .github/workflows/release_build.yml | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/.github/workflows/release_build.yml b/.github/workflows/release_build.yml index 4fa5520b1..3957d7cc0 100644 --- a/.github/workflows/release_build.yml +++ b/.github/workflows/release_build.yml @@ -116,13 +116,17 @@ jobs: - name: Build Library run: | - bazel build -c opt //:all + bazel build -c opt $(bazel query '//:all except tests(//:all)') - name: Build Examples run: | cd examples && bazel build -c opt //:all + - name: Run Unit Tests + run: | + bazel test -c opt //:all + - name: Install Test Files run: | mkdir test From 7920ad5624b9bf4fb2dc230b38836eebf432f9be Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Thu, 30 Apr 2026 11:41:26 -0400 Subject: [PATCH 41/52] Fixed unused argument warning in convert_time.cc example. --- examples/convert_time/convert_time.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/convert_time/convert_time.cc b/examples/convert_time/convert_time.cc index c207351da..81de48fa4 100644 --- a/examples/convert_time/convert_time.cc +++ b/examples/convert_time/convert_time.cc @@ -38,7 +38,7 @@ void PrintIMUMeasurement(const TimeProvider& time_provider, } /******************************************************************************/ -int main(int argc, const char* argv[]) { +int main(int /*argc*/, const char** /*argv*/) { // Define a time provider we'll use to convert between P1 and GPS time. TimeProvider time_provider; From ed988637f464c82216c12ad43952d36307a06898 Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Thu, 30 Apr 2026 11:45:01 -0400 Subject: [PATCH 42/52] Disabled CI matrix fail fast mode. --- .github/workflows/release_build.yml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.github/workflows/release_build.yml b/.github/workflows/release_build.yml index 3957d7cc0..d2e5ba08e 100644 --- a/.github/workflows/release_build.yml +++ b/.github/workflows/release_build.yml @@ -41,6 +41,7 @@ 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. @@ -151,6 +152,7 @@ jobs: name: CMake Build runs-on: ${{ matrix.os }} strategy: + fail-fast: false matrix: include: - os: ubuntu-latest @@ -333,6 +335,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: From 283b02a2b2a14913dbd31173d0a6afd0950d5ebf Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Thu, 30 Apr 2026 12:35:12 -0400 Subject: [PATCH 43/52] Use <> includes in fusion_engine_framer.h/cc. --- src/point_one/fusion_engine/parsers/fusion_engine_framer.cc | 6 +++--- src/point_one/fusion_engine/parsers/fusion_engine_framer.h | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) 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 { From 89e13ae995619ca2ed7c17220c1666ec0023f0e1 Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Thu, 30 Apr 2026 12:35:25 -0400 Subject: [PATCH 44/52] Use P1_EXPORT for TimeProvider for Windows. --- src/point_one/fusion_engine/utils/time_provider.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/point_one/fusion_engine/utils/time_provider.h b/src/point_one/fusion_engine/utils/time_provider.h index 9d7007315..04b4502a1 100644 --- a/src/point_one/fusion_engine/utils/time_provider.h +++ b/src/point_one/fusion_engine/utils/time_provider.h @@ -5,6 +5,7 @@ #pragma once +#include // For P1_EXPORT #include namespace point_one { @@ -14,7 +15,7 @@ namespace utils { /** * @brief Utility for converting between P1 and GPS time. */ -class TimeProvider { +class P1_EXPORT TimeProvider { public: TimeProvider() = default; From e158e0ed17dfa213b9362df64480d26a00158a5a Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Thu, 30 Apr 2026 12:45:00 -0400 Subject: [PATCH 45/52] Added note about gtest C++14 requirement. --- WORKSPACE | 2 ++ 1 file changed, 2 insertions(+) diff --git a/WORKSPACE b/WORKSPACE index 25fb4b7de..463fa6b8d 100644 --- a/WORKSPACE +++ b/WORKSPACE @@ -2,6 +2,8 @@ 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", From d5995caffe40e68afdbb22f08350488c498009d4 Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Thu, 30 Apr 2026 12:45:44 -0400 Subject: [PATCH 46/52] Use C++14 with Bazel 4.2.2 for gtest compatibility. --- .github/workflows/release_build.yml | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/.github/workflows/release_build.yml b/.github/workflows/release_build.yml index d2e5ba08e..17b6fc12a 100644 --- a/.github/workflows/release_build.yml +++ b/.github/workflows/release_build.yml @@ -45,10 +45,14 @@ jobs: 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 + cxxopt: --cxxopt=-std=c++14 # Run Bazel 6.5.0 on all compilers versions. - os: ubuntu-latest compiler: gcc @@ -117,16 +121,16 @@ jobs: - name: Build Library run: | - bazel build -c opt $(bazel query '//:all except tests(//:all)') + bazel build -c opt ${{ matrix.cxxopt }} $(bazel query '//:all except tests(//:all)') - name: Build Examples run: | cd examples && - bazel build -c opt //:all + bazel build -c opt ${{ matrix.cxxopt }} //:all - name: Run Unit Tests run: | - bazel test -c opt //:all + bazel test -c opt ${{ matrix.cxxopt }} //:all - name: Install Test Files run: | From dba69dd176351cb1c585d7fcfb35323b9c42b7f7 Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Thu, 30 Apr 2026 12:49:41 -0400 Subject: [PATCH 47/52] Cleaned up typo. --- .github/workflows/release_build.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/release_build.yml b/.github/workflows/release_build.yml index 17b6fc12a..d19e041c3 100644 --- a/.github/workflows/release_build.yml +++ b/.github/workflows/release_build.yml @@ -53,7 +53,7 @@ jobs: version: 11 bazel: 4.2.2 cxxopt: --cxxopt=-std=c++14 - # Run Bazel 6.5.0 on all compilers versions. + # Run Bazel 6.5.0 on all compiler versions. - os: ubuntu-latest compiler: gcc version: 9 From f8666fe488ac75a9aee505f5dc5d4e08ce004f10 Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Thu, 30 Apr 2026 12:49:52 -0400 Subject: [PATCH 48/52] Test library build for all supported C++ versions (11+). --- .github/workflows/release_build.yml | 27 +++++++++++++++++++++++++-- 1 file changed, 25 insertions(+), 2 deletions(-) diff --git a/.github/workflows/release_build.yml b/.github/workflows/release_build.yml index d19e041c3..9f60cd719 100644 --- a/.github/workflows/release_build.yml +++ b/.github/workflows/release_build.yml @@ -62,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 + cxxopt: --cxxopt=-std=c++11 - os: ubuntu-latest - compiler: clang - version: 14 + compiler: gcc + version: 11 + bazel: 6.5.0 + cxxopt: --cxxopt=-std=c++14 + - os: ubuntu-latest + compiler: gcc + version: 11 + bazel: 6.5.0 + cxxopt: --cxxopt=-std=c++17 + - os: ubuntu-latest + compiler: gcc + version: 11 + bazel: 6.5.0 + cxxopt: --cxxopt=-std=c++20 + - os: ubuntu-latest + compiler: gcc + version: 11 bazel: 6.5.0 + cxxopt: --cxxopt=-std=c++23 steps: - uses: actions/checkout@v2 @@ -129,6 +151,7 @@ jobs: bazel build -c opt ${{ matrix.cxxopt }} //:all - name: Run Unit Tests + if: "!contains(matrix.cxxopt, 'c++11')" run: | bazel test -c opt ${{ matrix.cxxopt }} //:all From c9b2964a11c7ea593a7587ec1c505551c054dd96 Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Thu, 30 Apr 2026 12:52:14 -0400 Subject: [PATCH 49/52] Moved --cxxopt setting. --- .github/workflows/release_build.yml | 26 ++++++++++++++++---------- 1 file changed, 16 insertions(+), 10 deletions(-) diff --git a/.github/workflows/release_build.yml b/.github/workflows/release_build.yml index 9f60cd719..c949cc6eb 100644 --- a/.github/workflows/release_build.yml +++ b/.github/workflows/release_build.yml @@ -52,7 +52,7 @@ jobs: compiler: gcc version: 11 bazel: 4.2.2 - cxxopt: --cxxopt=-std=c++14 + cppstd: c++14 # Run Bazel 6.5.0 on all compiler versions. - os: ubuntu-latest compiler: gcc @@ -71,27 +71,27 @@ jobs: compiler: gcc version: 11 bazel: 6.5.0 - cxxopt: --cxxopt=-std=c++11 + cppstd: c++11 - os: ubuntu-latest compiler: gcc version: 11 bazel: 6.5.0 - cxxopt: --cxxopt=-std=c++14 + cppstd: c++14 - os: ubuntu-latest compiler: gcc version: 11 bazel: 6.5.0 - cxxopt: --cxxopt=-std=c++17 + cppstd: c++17 - os: ubuntu-latest compiler: gcc version: 11 bazel: 6.5.0 - cxxopt: --cxxopt=-std=c++20 + cppstd: c++20 - os: ubuntu-latest compiler: gcc version: 11 bazel: 6.5.0 - cxxopt: --cxxopt=-std=c++23 + cppstd: c++23 steps: - uses: actions/checkout@v2 @@ -141,19 +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 ${{ matrix.cxxopt }} $(bazel query '//:all except tests(//:all)') + bazel build -c opt ${{ env.CXXOPT }} $(bazel query '//:all except tests(//:all)') - name: Build Examples run: | cd examples && - bazel build -c opt ${{ matrix.cxxopt }} //:all + bazel build -c opt ${{ env.CXXOPT }} //:all - name: Run Unit Tests - if: "!contains(matrix.cxxopt, 'c++11')" + if: matrix.cppstd != 'c++11' run: | - bazel test -c opt ${{ matrix.cxxopt }} //:all + bazel test -c opt ${{ env.CXXOPT }} //:all - name: Install Test Files run: | From 44521803bbe7c1f4bcad2625aa74a7e3f20d688d Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Thu, 30 Apr 2026 12:57:53 -0400 Subject: [PATCH 50/52] Fixed MSVC warning for private timestamp members in TimeProvider. --- src/point_one/fusion_engine/utils/time_provider.h | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/point_one/fusion_engine/utils/time_provider.h b/src/point_one/fusion_engine/utils/time_provider.h index 04b4502a1..22f8fae11 100644 --- a/src/point_one/fusion_engine/utils/time_provider.h +++ b/src/point_one/fusion_engine/utils/time_provider.h @@ -12,6 +12,14 @@ 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. */ @@ -61,6 +69,10 @@ class P1_EXPORT TimeProvider { messages::Timestamp prev_gps_time_; }; +#ifdef _MSC_VER +# pragma warning(pop) +#endif + } // namespace utils } // namespace fusion_engine } // namespace point_one From f8a374e5780902cf72e3d6a80952b8c95856c124 Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Thu, 30 Apr 2026 13:08:06 -0400 Subject: [PATCH 51/52] Added badges for C++ standard support. --- README.md | 1 + 1 file changed, 1 insertion(+) 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
From 18921afaf5be2101547c16101b76c8cb4bd6d417 Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Thu, 30 Apr 2026 14:34:32 -0400 Subject: [PATCH 52/52] Fixed unused imports. --- python/fusion_engine_client/utils/time_provider.py | 2 +- python/tests/test_timestamp.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/python/fusion_engine_client/utils/time_provider.py b/python/fusion_engine_client/utils/time_provider.py index 45260d312..33b139ff4 100644 --- a/python/fusion_engine_client/utils/time_provider.py +++ b/python/fusion_engine_client/utils/time_provider.py @@ -1,6 +1,6 @@ from typing import Optional, Union -from datetime import datetime, timezone, timedelta +from datetime import datetime, timedelta from gpstime import gpstime diff --git a/python/tests/test_timestamp.py b/python/tests/test_timestamp.py index 7c7481661..dfb12069d 100644 --- a/python/tests/test_timestamp.py +++ b/python/tests/test_timestamp.py @@ -1,4 +1,4 @@ -from datetime import datetime, timedelta, timezone +from datetime import datetime, timedelta import math import struct