Skip to content

fix(ubx): real UTC time and no fake sample timestamp on NAV-RELPOSNED/DAHEADING #240

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
dakejahl wants to merge 1 commit into
base: main
Choose a base branch
from
+42 −7
Draft

fix(ubx): real UTC time and no fake sample timestamp on NAV-RELPOSNED/DAHEADING #240

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
39 changes: 32 additions & 7 deletions src/ubx.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2617,12 +2617,15 @@ GPSDriverUBX::payloadRxDone()
timeinfo.tm_min = _buf.payload_rx_nav_pvt.min;
timeinfo.tm_sec = _buf.payload_rx_nav_pvt.sec;
_gps_position->time_utc_usec = timeFromUtc(timeinfo, _buf.payload_rx_nav_pvt.nano);
_last_utc_usec = _gps_position->time_utc_usec;
_last_utc_itow = _buf.payload_rx_nav_pvt.iTOW;

} else {
// The struct is reused across messages, so without this a receiver
// that lost time in a reset keeps reporting the last time it knew.
// 0 is the defined "unavailable" value.
_gps_position->time_utc_usec = 0;
_last_utc_usec = 0;
}

_gps_position->timestamp = gps_absolute_time();
Expand Down Expand Up @@ -2733,12 +2736,15 @@ GPSDriverUBX::payloadRxDone()
timeinfo.tm_min = _buf.payload_rx_nav_timeutc.min;
timeinfo.tm_sec = _buf.payload_rx_nav_timeutc.sec;
_gps_position->time_utc_usec = timeFromUtc(timeinfo, _buf.payload_rx_nav_timeutc.nano);
_last_utc_usec = _gps_position->time_utc_usec;
_last_utc_itow = _buf.payload_rx_nav_timeutc.iTOW;

} else {
// The struct is reused across messages, so without this a receiver
// that lost time in a reset keeps reporting the last time it knew.
// 0 is the defined "unavailable" value.
_gps_position->time_utc_usec = 0;
_last_utc_usec = 0;
}

_last_timestamp_time = gps_absolute_time();
Expand Down Expand Up @@ -2827,9 +2833,8 @@ GPSDriverUBX::payloadRxDone()

sensor_gnss_relative_s gps_rel{};

gps_rel.timestamp_sample = gps_absolute_time(); // TODO: adjust with delay estimate

gps_rel.time_utc_usec = _buf.payload_rx_nav_relposned.iTOW * 1000; // TODO: convert iTOW ms GPS time of week
// timestamp_sample is left unset: the receiver latency is not known here and is applied by the consumer
gps_rel.time_utc_usec = utcFromItow(_buf.payload_rx_nav_relposned.iTOW);
gps_rel.reference_station_id = _buf.payload_rx_nav_relposned.refStationId;

gps_rel.position[0] = (_buf.payload_rx_nav_relposned.relPosN + _buf.payload_rx_nav_relposned.relPosHPN * 1e-2f) * 1e-2f;
Expand Down Expand Up @@ -2890,10 +2895,8 @@ GPSDriverUBX::payloadRxDone()

sensor_gnss_relative_s gps_rel{};

gps_rel.timestamp_sample = gps_absolute_time();

// time_utc_usec is left at 0 (documented as unavailable): NAV-DAHEADING only carries
// iTOW, which cannot be converted to UTC without the week number and leap seconds.
// timestamp_sample is left unset: the receiver latency is not known here and is applied by the consumer
gps_rel.time_utc_usec = utcFromItow(_buf.payload_rx_nav_daheading.iTOW);

gps_rel.position[0] = _buf.payload_rx_nav_daheading.relPosN * 1e-3f; // mm -> m
gps_rel.position[1] = _buf.payload_rx_nav_daheading.relPosE * 1e-3f;
Expand Down Expand Up @@ -3180,6 +3183,28 @@ GPSDriverUBX::decodeInit()
_rx_payload_index = 0;
}

uint64_t
GPSDriverUBX::utcFromItow(uint32_t itow) const
{
if (_last_utc_usec == 0) {
return 0;
}

// GPS and UTC differ by a whole number of seconds, so the sub-week offset between epochs is the same in both
int64_t delta_ms = static_cast<int64_t>(itow) - static_cast<int64_t>(_last_utc_itow);

constexpr int64_t week_ms = 7 * 24 * 3600 * 1000LL;

if (delta_ms > week_ms / 2) {
delta_ms -= week_ms;

} else if (delta_ms < -week_ms / 2) {
delta_ms += week_ms;
}

return static_cast<uint64_t>(static_cast<int64_t>(_last_utc_usec) + delta_ms * 1000);
}

float
GPSDriverUBX::relPosHeadingToYaw(int32_t heading) const
{
Expand Down
10 changes: 10 additions & 0 deletions src/ubx.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1143,6 +1143,13 @@ class GPSDriverUBX : public GPSBaseStationSupport
*/
float relPosHeadingToYaw(int32_t heading) const;

/**
* UTC time of a navigation epoch, from the last message that carried both iTOW and a valid UTC time.
* @param itow GPS time of week [ms]
* @return UTC time [us], 0 while the receiver's time is not valid
*/
uint64_t utcFromItow(uint32_t itow) const;

/**
* While parsing add every byte (except the sync bytes) to the checksum
*/
Expand Down Expand Up @@ -1372,6 +1379,9 @@ class GPSDriverUBX : public GPSBaseStationSupport

uint64_t _last_timestamp_time{0};

uint64_t _last_utc_usec{0};
uint32_t _last_utc_itow{0};

Board _board{Board::unknown};

OutputMode _output_mode{OutputMode::GPS};
Expand Down
Loading