diff --git a/src/modules/sensors/vehicle_gps_position/gps_blending.cpp b/src/modules/sensors/vehicle_gps_position/gps_blending.cpp index 3673ca14bb6c..6582c37a240a 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 af5b1feab706..3c2fe326f08c 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); +}