From 24cee812795c669318d3e56e5df9261124e319a7 Mon Sep 17 00:00:00 2001 From: oystub Date: Thu, 7 Dec 2023 11:59:05 +0000 Subject: [PATCH] gps_blending: output valid time_utc_usec Before this fix, the time_utc_usec output from blending was always 0. This means that you wouldn't get a valid vehicle_gps_position/time_utc_usec With this commit, UTC timestamps are blended according to weights for all GPSes with a nonzero UTC timestamp value. It would be possible to simply use the first valid UTC timestamp instead of blending, but since the system timestamps are blended, it seems suitable to blend UTC timestamps as well. --- .../vehicle_gps_position/gps_blending.cpp | 15 ++++++++++ .../gps_blending_test.cpp | 28 +++++++++++++++++++ 2 files changed, 43 insertions(+) diff --git a/src/modules/sensors/vehicle_gps_position/gps_blending.cpp b/src/modules/sensors/vehicle_gps_position/gps_blending.cpp index 3673ca14bb..6582c37a24 100644 --- a/src/modules/sensors/vehicle_gps_position/gps_blending.cpp +++ b/src/modules/sensors/vehicle_gps_position/gps_blending.cpp @@ -493,6 +493,21 @@ sensor_gps_s GpsBlending::gps_blend_states(float blend_weights[GPS_MAX_RECEIVERS gps_blended_state.heading_accuracy = _gps_state[gps_best_yaw_index].heading_accuracy; } + // Blend UTC timestamp from all receivers that are publishing a valid time_utc_usec value + double utc_weight_sum = 0.0; + double utc_time_sum = 0.0; + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { + if (_gps_state[i].time_utc_usec > 0) { + utc_time_sum += (double)_gps_state[i].time_utc_usec * (double)blend_weights[i]; + utc_weight_sum += (double)blend_weights[i]; + } + } + + if (utc_weight_sum > 0.0) { + gps_blended_state.time_utc_usec = (uint64_t)(utc_time_sum / utc_weight_sum); + } + return gps_blended_state; } diff --git a/src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp b/src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp index af5b1feab7..3c2fe326f0 100644 --- a/src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp +++ b/src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp @@ -275,3 +275,31 @@ TEST_F(GpsBlendingTest, dualReceiverFailover) EXPECT_EQ(gps_blending.getSelectedGps(), 0); EXPECT_TRUE(gps_blending.isNewOutputDataAvailable()); } + +TEST_F(GpsBlendingTest, dualReceiverUTCTime) +{ + GpsBlending gps_blending; + sensor_gps_s gps_data0 = getDefaultGpsData(); + sensor_gps_s gps_data1 = getDefaultGpsData(); + + // WHEN: Only GPS1 has a nonzero UTC time + gps_blending = GpsBlending(); + gps_data1.time_utc_usec = 1700000000000000ULL; + gps_blending.setGpsData(gps_data0, 0); + gps_blending.setGpsData(gps_data1, 1); + gps_blending.setBlendingUseHPosAccuracy(true); + gps_blending.update(_time_now_us); + // THEN: GPS 1 time should be used + EXPECT_EQ(gps_blending.getOutputGpsData().time_utc_usec, gps_data1.time_utc_usec); + + // WHEN: Both GPSes have a nonzero UTC time + gps_blending = GpsBlending(); + gps_data0.time_utc_usec = 1700000000001000ULL; + gps_data1.time_utc_usec = 1700000000000000ULL; + gps_blending.setGpsData(gps_data0, 0); + gps_blending.setGpsData(gps_data1, 1); + gps_blending.setBlendingUseHPosAccuracy(true); + gps_blending.update(_time_now_us); + // THEN: The average of the two timestamps should be used + EXPECT_EQ(gps_blending.getOutputGpsData().time_utc_usec, 1700000000000500ULL); +}