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.

Author: oystub

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;
 }
 

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);
+}