diff --git a/src/ashtech.cpp b/src/ashtech.cpp index 859e91d..1a23598 100644 --- a/src/ashtech.cpp +++ b/src/ashtech.cpp @@ -137,34 +137,16 @@ int GPSDriverAshtech::handleMessage(int len) timeinfo.tm_sec = int(ashtech_sec); timeinfo.tm_isdst = 0; -#ifndef NO_MKTIME time_t epoch = mktime(&timeinfo); if (epoch > GPS_EPOCH_SECS) { uint64_t usecs = static_cast((ashtech_sec - static_cast(ashtech_sec))) * 1000000; - - // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it - // and control its drift. Since we rely on the HRT for our monotonic - // clock, updating it from time to time is safe. - - timespec ts{}; - ts.tv_sec = epoch; - ts.tv_nsec = usecs * 1000; - - setClock(ts); - _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; _gps_position->time_utc_usec += usecs; } else { _gps_position->time_utc_usec = 0; } - -#else - _gps_position->time_utc_usec = 0; -#endif - - _last_timestamp_time = gps_absolute_time(); } else if ((memcmp(_rx_buffer + 3, "GGA,", 3) == 0) && (uiCalcComma == 14) && !_got_pashr_pos_message) { @@ -262,7 +244,7 @@ int GPSDriverAshtech::handleMessage(int len) _gps_position->fix_type = 3 + fix_quality - 1; } - _gps_position->timestamp = gps_absolute_time(); + _gps_position->timestamp_sample = gps_absolute_time(); _gps_position->vel_m_s = 0; /**< GPS ground speed (m/s) */ _gps_position->vel_n_m_s = 0; /**< GPS ground speed in m/s */ @@ -444,7 +426,7 @@ int GPSDriverAshtech::handleMessage(int len) } } - _gps_position->timestamp = gps_absolute_time(); + _gps_position->timestamp_sample = gps_absolute_time(); float track_rad = static_cast(track_true) * M_PI_F / 180.0f; @@ -705,11 +687,6 @@ int GPSDriverAshtech::handleMessage(int len) } - if (ret == 1) { - _gps_position->timestamp_time_relative = (int32_t)(_last_timestamp_time - _gps_position->timestamp); - } - - // handle survey-in status update if (_survey_in_start != 0) { const gps_abstime now = gps_absolute_time(); diff --git a/src/ashtech.h b/src/ashtech.h index 2db2925..abb9f03 100644 --- a/src/ashtech.h +++ b/src/ashtech.h @@ -128,7 +128,6 @@ class GPSDriverAshtech : public GPSBaseStationSupport uint8_t _rx_buffer[ASHTECH_RECV_BUFFER_SIZE]; uint16_t _rx_buffer_bytes{}; - uint64_t _last_timestamp_time{0}; float _heading_offset; diff --git a/src/emlid_reach.cpp b/src/emlid_reach.cpp index 75f774a..1370973 100644 --- a/src/emlid_reach.cpp +++ b/src/emlid_reach.cpp @@ -282,7 +282,7 @@ GPSDriverEmlidReach::handleErbSentence() } else if (_erb_buff.header.id == ERB_ID_GEODIC_POSITION) { - _gps_position->timestamp = gps_absolute_time(); + _gps_position->timestamp_sample = gps_absolute_time(); _last_POS_timeGPS = _erb_buff.payload.geodic_position.timeGPS; _gps_position->lon = round(_erb_buff.payload.geodic_position.longitude * 1e7); diff --git a/src/femtomes.cpp b/src/femtomes.cpp index 71c7da8..0184851 100644 --- a/src/femtomes.cpp +++ b/src/femtomes.cpp @@ -114,7 +114,6 @@ int GPSDriverFemto::handleMessage(int len) _gps_position->vel_e_m_s = _femto_uav_gps.vel_e_m_s; _gps_position->vel_d_m_s = _femto_uav_gps.vel_d_m_s; _gps_position->cog_rad = _femto_uav_gps.cog_rad; - _gps_position->timestamp_time_relative = _femto_uav_gps.timestamp_time_relative; _gps_position->fix_type = _femto_uav_gps.fix_type; _gps_position->vel_ned_valid = _femto_uav_gps.vel_ned_valid; _gps_position->satellites_used = _femto_uav_gps.satellites_used; @@ -134,7 +133,7 @@ int GPSDriverFemto::handleMessage(int len) _gps_position->heading = NAN; } - _gps_position->timestamp = gps_absolute_time(); + _gps_position->timestamp_sample = gps_absolute_time(); ret = 1; diff --git a/src/femtomes.h b/src/femtomes.h index 27d24b0..76d4623 100644 --- a/src/femtomes.h +++ b/src/femtomes.h @@ -66,7 +66,6 @@ typedef struct { float vel_e_m_s; /** GPS East velocity, (metres/sec)*/ float vel_d_m_s; /** GPS Down velocity, (metres/sec)*/ float cog_rad; /** Course over ground (NOT heading, but direction of movement), -PI..PI, (radians)*/ - int32_t timestamp_time_relative;/** timestamp + timestamp_time_relative = Time of the UTC timestamp since system start, (microseconds)*/ float heading; /** heading angle of XYZ body frame rel to NED. Set to NaN if not available and updated (used for dual antenna GPS), (rad, [-PI, PI])*/ uint8_t fix_type; /** 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic, float, 6: Real-Time Kinematic, fixed, 8: Extrapolated. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.*/ bool vel_ned_valid; /** True if NED velocity is valid*/ diff --git a/src/gps_helper.h b/src/gps_helper.h index dc15491..79a036e 100644 --- a/src/gps_helper.h +++ b/src/gps_helper.h @@ -93,14 +93,6 @@ enum class GPSCallbackType { * return: ignored */ surveyInStatus, - - /** - * can be used to set the current clock accurately - * data1: pointer to a timespec struct - * data2: ignored - * return: ignored - */ - setClock, }; enum class GPSRestartType { @@ -271,11 +263,6 @@ class GPSHelper _callback(GPSCallbackType::gotRTCMMessage, buf, buf_length, _callback_user); } - void setClock(timespec &t) - { - _callback(GPSCallbackType::setClock, &t, 0, _callback_user); - } - /** * Convert an ECEF (Earth Centered Earth Fixed) coordinate to LLA WGS84 (Lat, Lon, Alt). * Ported from: https://stackoverflow.com/a/25428344 diff --git a/src/mtk.cpp b/src/mtk.cpp index 11ac27e..4c253f9 100644 --- a/src/mtk.cpp +++ b/src/mtk.cpp @@ -264,21 +264,9 @@ GPSDriverMTK::handleMessage(gps_mtk_packet_t &packet) timeinfo.tm_isdst = 0; -#ifndef NO_MKTIME - time_t epoch = mktime(&timeinfo); if (epoch > GPS_EPOCH_SECS) { - // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it - // and control its drift. Since we rely on the HRT for our monotonic - // clock, updating it from time to time is safe. - - timespec ts{}; - ts.tv_sec = epoch; - ts.tv_nsec = timeinfo_conversion_temp * 1000000ULL; - - setClock(ts); - _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; _gps_position->time_utc_usec += timeinfo_conversion_temp * 1000ULL; @@ -286,12 +274,7 @@ GPSDriverMTK::handleMessage(gps_mtk_packet_t &packet) _gps_position->time_utc_usec = 0; } -#else - _gps_position->time_utc_usec = 0; -#endif - - _gps_position->timestamp = gps_absolute_time(); - _gps_position->timestamp_time_relative = 0; + _gps_position->timestamp_sample = gps_absolute_time(); // Position and velocity update always at the same time _rate_count_vel++; diff --git a/src/nmea.cpp b/src/nmea.cpp index 5067c97..409a7ba 100644 --- a/src/nmea.cpp +++ b/src/nmea.cpp @@ -142,10 +142,8 @@ int GPSDriverNMEA::handleMessage(int len) int utc_minute = static_cast((utc_time - utc_hour * 10000) / 100); double utc_sec = static_cast(utc_time - utc_hour * 10000 - utc_minute * 100); - /* - * convert to unix timestamp - */ - struct tm timeinfo = {}; + // convert to unix timestamp + struct tm timeinfo {}; timeinfo.tm_year = year - 1900; timeinfo.tm_mon = month - 1; timeinfo.tm_mday = day; @@ -154,24 +152,11 @@ int GPSDriverNMEA::handleMessage(int len) timeinfo.tm_sec = int(utc_sec); timeinfo.tm_isdst = 0; -#ifndef NO_MKTIME time_t epoch = mktime(&timeinfo); if (epoch > GPS_EPOCH_SECS) { uint64_t usecs = static_cast((utc_sec - static_cast(utc_sec)) * 1000000); - // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it - // and control its drift. Since we rely on the HRT for our monotonic - // clock, updating it from time to time is safe. - - if (!_clock_set) { - timespec ts{}; - ts.tv_sec = epoch; - ts.tv_nsec = usecs * 1000; - setClock(ts); - _clock_set = true; - } - _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; _gps_position->time_utc_usec += usecs; @@ -179,11 +164,8 @@ int GPSDriverNMEA::handleMessage(int len) _gps_position->time_utc_usec = 0; } -#else - _gps_position->time_utc_usec = 0; -#endif _TIME_received = true; - _gps_position->timestamp = gps_absolute_time(); + _gps_position->timestamp_sample = gps_absolute_time(); } else if ((memcmp(_rx_buffer + 3, "GGA,", 4) == 0) && (uiCalcComma >= 14)) { /* @@ -300,7 +282,7 @@ int GPSDriverNMEA::handleMessage(int len) _FIX_received = true; _gps_position->c_variance_rad = 0.1f; - _gps_position->timestamp = gps_absolute_time(); + _gps_position->timestamp_sample = gps_absolute_time(); } else if (memcmp(_rx_buffer + 3, "HDT,", 4) == 0 && uiCalcComma == 2) { /* @@ -506,13 +488,12 @@ int GPSDriverNMEA::handleMessage(int len) _gps_position->vel_ned_valid = true; /**< Flag to indicate if NED speed is valid */ _gps_position->c_variance_rad = 0.1f; _gps_position->s_variance_m_s = 0; - _gps_position->timestamp = gps_absolute_time(); - _last_timestamp_time = gps_absolute_time(); + _gps_position->timestamp_sample = gps_absolute_time(); /* * convert to unix timestamp */ - struct tm timeinfo = {}; + struct tm timeinfo {}; timeinfo.tm_year = nmea_year + 100; timeinfo.tm_mon = nmea_mth - 1; timeinfo.tm_mday = nmea_day; @@ -521,24 +502,10 @@ int GPSDriverNMEA::handleMessage(int len) timeinfo.tm_sec = int(utc_sec); timeinfo.tm_isdst = 0; -#ifndef NO_MKTIME time_t epoch = mktime(&timeinfo); if (epoch > GPS_EPOCH_SECS) { uint64_t usecs = static_cast((utc_sec - static_cast(utc_sec)) * 1000000); - - // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it - // and control its drift. Since we rely on the HRT for our monotonic - // clock, updating it from time to time is safe. - if (!_clock_set) { - timespec ts{}; - ts.tv_sec = epoch; - ts.tv_nsec = usecs * 1000; - - setClock(ts); - _clock_set = true; - } - _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; _gps_position->time_utc_usec += usecs; @@ -546,10 +513,6 @@ int GPSDriverNMEA::handleMessage(int len) _gps_position->time_utc_usec = 0; } -#else - _gps_position->time_utc_usec = 0; -#endif - if (!_POS_received && (_last_POS_timeUTC < utc_time)) { _last_POS_timeUTC = utc_time; _POS_received = true; @@ -859,8 +822,6 @@ int GPSDriverNMEA::handleMessage(int len) if (_VEL_received && _POS_received) { ret = 1; - _gps_position->timestamp_time_relative = (int32_t)(_last_timestamp_time - _gps_position->timestamp); - _clock_set = false; _VEL_received = false; _POS_received = false; _rate_count_vel++; diff --git a/src/nmea.h b/src/nmea.h index e0bd13d..ccd06d3 100644 --- a/src/nmea.h +++ b/src/nmea.h @@ -90,7 +90,6 @@ class GPSDriverNMEA : public GPSHelper double _last_POS_timeUTC{0}; double _last_VEL_timeUTC{0}; double _last_FIX_timeUTC{0}; - uint64_t _last_timestamp_time{0}; uint8_t _sat_num_gga{0}; uint8_t _sat_num_gns{0}; @@ -101,9 +100,7 @@ class GPSDriverNMEA : public GPSHelper uint8_t _sat_num_gbgsv{0}; uint8_t _sat_num_bdgsv{0}; - bool _clock_set {false}; - -// check if we got all basic essential packages we need + // check if we got all basic essential packages we need bool _TIME_received{false}; bool _POS_received{false}; bool _ALT_received{false}; diff --git a/src/sbf.cpp b/src/sbf.cpp index a05e22f..10ca51c 100644 --- a/src/sbf.cpp +++ b/src/sbf.cpp @@ -387,8 +387,6 @@ int // 0 = no message handled, 1 = message handled, 2 = sat info message hand GPSDriverSBF::payloadRxDone() { int ret = 0; - struct tm timeinfo; - time_t epoch; if (_buf.length <= 4 || _buf.crc16 != crc16(reinterpret_cast(&_buf) + 4, _buf.length - 4)) { return 1; @@ -474,10 +472,9 @@ GPSDriverSBF::payloadRxDone() } _gps_position->time_utc_usec = 0; -#ifndef NO_MKTIME - /* convert to unix timestamp */ - memset(&timeinfo, 0, sizeof(timeinfo)); + /* convert to unix timestamp */ + struct tm timeinfo {}; timeinfo.tm_year = 1980 - 1900; timeinfo.tm_mon = 0; timeinfo.tm_mday = 6 + _buf.WNc * 7; @@ -485,26 +482,14 @@ GPSDriverSBF::payloadRxDone() timeinfo.tm_min = 0; timeinfo.tm_sec = _buf.TOW / 1000; - epoch = mktime(&timeinfo); + time_t epoch = mktime(&timeinfo); if (epoch > GPS_EPOCH_SECS) { - // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it - // and control its drift. Since we rely on the HRT for our monotonic - // clock, updating it from time to time is safe. - - timespec ts; - memset(&ts, 0, sizeof(ts)); - ts.tv_sec = epoch; - ts.tv_nsec = (_buf.TOW % 1000) * 1000 * 1000; - setClock(ts); - _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; _gps_position->time_utc_usec += (_buf.TOW % 1000) * 1000; } -#endif - _gps_position->timestamp = gps_absolute_time(); - _last_timestamp_time = _gps_position->timestamp; + _gps_position->timestamp_sample = gps_absolute_time(); _rate_count_vel++; _rate_count_lat_lon++; ret |= (_msg_status == 7) ? 1 : 0; @@ -536,10 +521,6 @@ GPSDriverSBF::payloadRxDone() break; } - if (ret > 0) { - _gps_position->timestamp_time_relative = static_cast(_last_timestamp_time - _gps_position->timestamp); - } - if (ret == 1) { _msg_status &= ~1; } diff --git a/src/sbf.h b/src/sbf.h index 8699217..fad56ba 100644 --- a/src/sbf.h +++ b/src/sbf.h @@ -356,7 +356,6 @@ class GPSDriverSBF : public GPSBaseStationSupport sensor_gps_s *_gps_position { nullptr }; satellite_info_s *_satellite_info { nullptr }; uint8_t _dynamic_model{ 7 }; - uint64_t _last_timestamp_time { 0 }; bool _configured { false }; uint8_t _msg_status { 0 }; sbf_decode_state_t _decode_state { SBF_DECODE_SYNC1 }; diff --git a/src/ubx.cpp b/src/ubx.cpp index 5109e9e..9d724a4 100644 --- a/src/ubx.cpp +++ b/src/ubx.cpp @@ -337,6 +337,8 @@ GPSDriverUBX::configure(unsigned &baudrate, const GPSConfig &config) ret = configureDevicePreV27(config.gnss_systems); } + _baudrate = baudrate; // store final baudrate + if (ret != 0) { return ret; } @@ -1236,6 +1238,22 @@ GPSDriverUBX::payloadRxInit() } else if (!_use_nav_pvt) { _rx_state = UBX_RXMSG_DISABLE; // disable if not using NAV-PVT + + } else { + _gps_position->timestamp_sample = gps_absolute_time(); + + // adjust timestamp sample to account for transfer time + if (_baudrate != 0) { + float character_rate = _baudrate / 10; // 10 bits per character + float bytes_per_s = character_rate / 8; // 8 characters per byte + double transit_time_s = _rx_payload_length / bytes_per_s; + uint64_t transit_time_us = transit_time_s * 1e6; + + //printf("baudrate: %d, transit time %.6f s (%llu) SZ:%d\n", _baudrate, transit_time_s, transit_time_us, _rx_payload_length); + if (_gps_position->timestamp_sample > transit_time_us) { + _gps_position->timestamp_sample -= transit_time_us; + } + } } break; @@ -1259,6 +1277,9 @@ GPSDriverUBX::payloadRxInit() } else if (_use_nav_pvt) { _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead + + } else { + _gps_position->timestamp_sample = gps_absolute_time(); } break; @@ -1272,6 +1293,9 @@ GPSDriverUBX::payloadRxInit() } else if (_use_nav_pvt) { _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead + + } else { + _gps_position->timestamp_sample = gps_absolute_time(); } break; @@ -1805,6 +1829,7 @@ GPSDriverUBX::payloadRxDone() if ((_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_VALIDDATE) && (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_VALIDTIME) && (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_FULLYRESOLVED)) { + /* convert to unix timestamp */ tm timeinfo{}; timeinfo.tm_year = _buf.payload_rx_nav_pvt.year - 1900; @@ -1814,35 +1839,17 @@ GPSDriverUBX::payloadRxDone() timeinfo.tm_min = _buf.payload_rx_nav_pvt.min; timeinfo.tm_sec = _buf.payload_rx_nav_pvt.sec; -#ifndef NO_MKTIME time_t epoch = mktime(&timeinfo); if (epoch > GPS_EPOCH_SECS) { - // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it - // and control its drift. Since we rely on the HRT for our monotonic - // clock, updating it from time to time is safe. - - timespec ts{}; - ts.tv_sec = epoch; - ts.tv_nsec = _buf.payload_rx_nav_pvt.nano; - - setClock(ts); - _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; _gps_position->time_utc_usec += _buf.payload_rx_nav_pvt.nano / 1000; } else { _gps_position->time_utc_usec = 0; } - -#else - _gps_position->time_utc_usec = 0; -#endif } - _gps_position->timestamp = gps_absolute_time(); - _last_timestamp_time = _gps_position->timestamp; - _rate_count_vel++; _rate_count_lat_lon++; @@ -1878,8 +1885,6 @@ GPSDriverUBX::payloadRxDone() _gps_position->epv = static_cast(_buf.payload_rx_nav_posllh.vAcc) * 1e-3f; // from mm to m _gps_position->alt_ellipsoid = _buf.payload_rx_nav_posllh.height; - _gps_position->timestamp = gps_absolute_time(); - _rate_count_lat_lon++; _got_posllh = true; @@ -1907,6 +1912,7 @@ GPSDriverUBX::payloadRxDone() case UBX_MSG_NAV_TIMEUTC: UBX_TRACE_RXMSG("Rx NAV-TIMEUTC"); + fprintf(stderr, "UBX_MSG_NAV_TIMEUTC\n"); if (_buf.payload_rx_nav_timeutc.valid & UBX_RX_NAV_TIMEUTC_VALID_VALIDUTC) { // convert to unix timestamp @@ -1918,36 +1924,19 @@ GPSDriverUBX::payloadRxDone() timeinfo.tm_min = _buf.payload_rx_nav_timeutc.min; timeinfo.tm_sec = _buf.payload_rx_nav_timeutc.sec; timeinfo.tm_isdst = 0; -#ifndef NO_MKTIME + time_t epoch = mktime(&timeinfo); // only set the time if it makes sense - if (epoch > GPS_EPOCH_SECS) { - // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it - // and control its drift. Since we rely on the HRT for our monotonic - // clock, updating it from time to time is safe. - - timespec ts{}; - ts.tv_sec = epoch; - ts.tv_nsec = _buf.payload_rx_nav_timeutc.nano; - - setClock(ts); - _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; _gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000; } else { _gps_position->time_utc_usec = 0; } - -#else - _gps_position->time_utc_usec = 0; -#endif } - _last_timestamp_time = gps_absolute_time(); - ret = 1; break; @@ -2114,10 +2103,6 @@ GPSDriverUBX::payloadRxDone() break; } - if (ret > 0) { - _gps_position->timestamp_time_relative = (int32_t)(_last_timestamp_time - _gps_position->timestamp); - } - return ret; } diff --git a/src/ubx.h b/src/ubx.h index d51f73d..2c56ded 100644 --- a/src/ubx.h +++ b/src/ubx.h @@ -1097,8 +1097,6 @@ class GPSDriverUBX : public GPSBaseStationSupport uint32_t _ubx_version{0}; - uint64_t _last_timestamp_time{0}; - Board _board{Board::unknown}; OutputMode _output_mode{OutputMode::GPS}; @@ -1107,6 +1105,8 @@ class GPSDriverUBX : public GPSBaseStationSupport const UBXMode _mode; const float _heading_offset; + + unsigned _baudrate{0}; };