From 8337288d166ed8d91fe4eb50d93f17830fe3fa6d Mon Sep 17 00:00:00 2001 From: Dmitry Ponomarev <36133264+PonomarevDA@users.noreply.github.com> Date: Thu, 14 Nov 2024 20:10:33 +0300 Subject: [PATCH] add simulated gnss utc timestamp (#18) --- src/autopilot_interface/cyphal_hitl.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/src/autopilot_interface/cyphal_hitl.cpp b/src/autopilot_interface/cyphal_hitl.cpp index 2b84028..b08f9e2 100644 --- a/src/autopilot_interface/cyphal_hitl.cpp +++ b/src/autopilot_interface/cyphal_hitl.cpp @@ -79,6 +79,16 @@ void CyphalHitlInterface::publish_magnetometer(const Vector3 magnetic_field_gaus magnetometer.publish(magnetic_field); } +// Simulate GNSS timestamp by wrapping the time to align with a typical GNSS time reference (e.g., GPS epoch). +// GPS epoch started on January 6, 1980, so we calculate the time since then. +uint64_t simulate_gnss_utc_timestamp_usec() { + auto now = std::chrono::system_clock::now(); + auto duration_since_epoch = std::chrono::duration_cast(now.time_since_epoch()); + constexpr uint64_t gps_epoch_offset_usec = 315964800000000ULL; + uint64_t gnss_utc_timestamp_usec = duration_since_epoch.count() - gps_epoch_offset_usec; + return gnss_utc_timestamp_usec; +} + void CyphalHitlInterface::publish_gnss(const Vector3& global_pose, const Vector3& ned_velocity) { uint32_t crnt_time = HAL_GetTick() % 60000; float time_multiplier; @@ -91,6 +101,11 @@ void CyphalHitlInterface::publish_gnss(const Vector3& global_pose, const Vector3 float random_offset = (std::rand() % 100 - 50) * 0.0002; random_multiplier = std::clamp(random_multiplier + random_offset, 0.85f, 1.15f); + auto gnss_utc_timestamp_usec = simulate_gnss_utc_timestamp_usec(); + uint64_t MICROSECONDS_IN_WEEK = 604800000000; // 7*24*60*60*1e6; + gps_gnss.msg.time_week = static_cast(gnss_utc_timestamp_usec / MICROSECONDS_IN_WEEK); + gps_gnss.msg.time_week_ms = static_cast((gnss_utc_timestamp_usec % MICROSECONDS_IN_WEEK) / 1000); + gps_gnss.msg.point.latitude = global_pose[0] * RAD_TO_DEGREE; gps_gnss.msg.point.longitude = global_pose[1] * RAD_TO_DEGREE; gps_gnss.msg.point.altitude.meter = global_pose[2];