From efc36c926c14d0df504b9bddbcb85ad80abb841f Mon Sep 17 00:00:00 2001 From: angpre <angelo02prete@gmail.com> Date: Mon, 29 May 2023 21:11:25 +0200 Subject: [PATCH] [UBXGPS] Added utc timestamp --- src/shared/sensors/UBXGPS/UBXGPSData.h | 43 +++++++++++++++++++++- src/shared/sensors/UBXGPS/UBXGPSSerial.cpp | 8 ++++ src/shared/sensors/UBXGPS/UBXGPSSpi.cpp | 8 ++++ src/tests/sensors/test-ubxgps-serial.cpp | 7 +++- 4 files changed, 62 insertions(+), 4 deletions(-) diff --git a/src/shared/sensors/UBXGPS/UBXGPSData.h b/src/shared/sensors/UBXGPS/UBXGPSData.h index ed734d258..43d0df2a9 100644 --- a/src/shared/sensors/UBXGPS/UBXGPSData.h +++ b/src/shared/sensors/UBXGPS/UBXGPSData.h @@ -27,13 +27,48 @@ namespace Boardcore { +/** + * @brief Structure to handle UBX UTC time. + * Nanoseconds range from -5000000 (5 ms) to 994999999 (~995 ms) and when + * negative the other fields have been rounded to the nearest hundredth of a + * second. + * Because of leap seconds, minutes can be a second longer or shorter, so + * seconds range from 0 to 60. + */ +struct UBXDateTime +{ + uint16_t year = 0; // Year (UTC) [y] + uint8_t month = 0; // Month, range 1..12 (UTC) [month] + uint8_t day = 0; // Day of month, range 1..31 (UTC) [d] + uint8_t hour = 0; // Hour of day, range 0..23 (UTC) [h] + uint8_t minute = 0; // Minute of hour, range 0..59 (UTC) [min] + uint8_t second = 0; // Seconds of minute, range 0..60 (UTC) [s] + int32_t nanosecond = 0; // Fraction of second, range -1e9 .. 1e9 (UTC) [ns] + uint32_t accuracy = 0; // Time accuracy estimate (UTC) [ns] + + static std::string header() + { + return "year,month,day,hour,minute,second,nanosecond,accuracy"; + } + + void print(std::ostream &os) const + { + os << year << "," << (int)month << "," << (int)day << "," << (int)hour + << "," << (int)minute << "," << (int)second << "," << nanosecond + << "," << accuracy; + } +}; + struct UBXGPSData : public GPSData { + UBXDateTime ubxTime; + static std::string header() { return "gpsTimestamp,latitude,longitude,height,velocityNorth," "velocityEast,velocityDown,speed,track,positionDOP,satellites," - "fix\n"; + "fix," + + UBXDateTime::header() + "\n"; } void print(std::ostream &os) const @@ -41,7 +76,11 @@ struct UBXGPSData : public GPSData os << gpsTimestamp << "," << latitude << "," << longitude << "," << height << "," << velocityNorth << "," << velocityEast << "," << velocityDown << "," << speed << "," << track << "," << positionDOP - << "," << (int)satellites << "," << (int)fix << "\n"; + << "," << (int)satellites << "," << (int)fix << ","; + + ubxTime.print(os); + + os << "\n"; } }; diff --git a/src/shared/sensors/UBXGPS/UBXGPSSerial.cpp b/src/shared/sensors/UBXGPS/UBXGPSSerial.cpp index 0cfb238df..957f0a04a 100644 --- a/src/shared/sensors/UBXGPS/UBXGPSSerial.cpp +++ b/src/shared/sensors/UBXGPS/UBXGPSSerial.cpp @@ -381,6 +381,14 @@ void UBXGPSSerial::run() threadSample.positionDOP = (float)pvtP.pDOP / 1e2; threadSample.satellites = pvtP.numSV; threadSample.fix = pvtP.fixType; + threadSample.ubxTime = {.year = pvtP.year, + .month = pvtP.month, + .day = pvtP.day, + .hour = pvtP.hour, + .minute = pvtP.min, + .second = pvtP.sec, + .nanosecond = pvtP.nano, + .accuracy = pvtP.tAcc}; StackLogger::getInstance().updateStack(THID_GPS); } diff --git a/src/shared/sensors/UBXGPS/UBXGPSSpi.cpp b/src/shared/sensors/UBXGPS/UBXGPSSpi.cpp index f01a045a9..d62ace27f 100644 --- a/src/shared/sensors/UBXGPS/UBXGPSSpi.cpp +++ b/src/shared/sensors/UBXGPS/UBXGPSSpi.cpp @@ -130,6 +130,14 @@ UBXGPSData UBXGPSSpi::sampleImpl() sample.positionDOP = (float)pvtP.pDOP / 1e2; sample.satellites = pvtP.numSV; sample.fix = pvtP.fixType; + sample.ubxTime = {.year = pvtP.year, + .month = pvtP.month, + .day = pvtP.day, + .hour = pvtP.hour, + .minute = pvtP.min, + .second = pvtP.sec, + .nanosecond = pvtP.nano, + .accuracy = pvtP.tAcc}; return sample; } diff --git a/src/tests/sensors/test-ubxgps-serial.cpp b/src/tests/sensors/test-ubxgps-serial.cpp index 063bf6df8..722cbef73 100644 --- a/src/tests/sensors/test-ubxgps-serial.cpp +++ b/src/tests/sensors/test-ubxgps-serial.cpp @@ -81,10 +81,13 @@ int main() printf( "[gps] timestamp: % 4.3f, fix: %01d lat: % f lon: % f " "height: %4.1f nsat: %2d speed: %3.2f velN: % 3.2f velE: % 3.2f " - "track %3.1f\n", + "track %3.1f utc: %04u:%02u:%02u:%02u:%02u:%02u:%09ld\n", (float)dataGPS.gpsTimestamp / 1000000, dataGPS.fix, dataGPS.latitude, dataGPS.longitude, dataGPS.height, dataGPS.satellites, dataGPS.speed, dataGPS.velocityNorth, - dataGPS.velocityEast, dataGPS.track); + dataGPS.velocityEast, dataGPS.track, dataGPS.ubxTime.year, + dataGPS.ubxTime.month, dataGPS.ubxTime.day, dataGPS.ubxTime.hour, + dataGPS.ubxTime.minute, dataGPS.ubxTime.second, + dataGPS.ubxTime.nanosecond); } } -- GitLab