dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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.
This commit is contained in:
oystub 2023-12-07 11:59:05 +00:00 committed by Daniel Agar
parent c769fc7785
commit 24cee81279
2 changed files with 43 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/modules/sensors/vehicle_gps_position/gps_blending.cpp
View File

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

28
src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp
View File

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