Skip to content

Commit c1ba66f

Browse files
committed
remove setClock() callback and add timestamp_sample
1 parent 6fcf068 commit c1ba66f

13 files changed

+28
-178
lines changed

src/ashtech.cpp

+2-25
Original file line numberDiff line numberDiff line change
@@ -137,34 +137,16 @@ int GPSDriverAshtech::handleMessage(int len)
137137
timeinfo.tm_sec = int(ashtech_sec);
138138
timeinfo.tm_isdst = 0;
139139

140-
#ifndef NO_MKTIME
141140
time_t epoch = mktime(&timeinfo);
142141

143142
if (epoch > GPS_EPOCH_SECS) {
144143
uint64_t usecs = static_cast<uint64_t>((ashtech_sec - static_cast<uint64_t>(ashtech_sec))) * 1000000;
145-
146-
// FMUv2+ boards have a hardware RTC, but GPS helps us to configure it
147-
// and control its drift. Since we rely on the HRT for our monotonic
148-
// clock, updating it from time to time is safe.
149-
150-
timespec ts{};
151-
ts.tv_sec = epoch;
152-
ts.tv_nsec = usecs * 1000;
153-
154-
setClock(ts);
155-
156144
_gps_position->time_utc_usec = static_cast<uint64_t>(epoch) * 1000000ULL;
157145
_gps_position->time_utc_usec += usecs;
158146

159147
} else {
160148
_gps_position->time_utc_usec = 0;
161149
}
162-
163-
#else
164-
_gps_position->time_utc_usec = 0;
165-
#endif
166-
167-
_last_timestamp_time = gps_absolute_time();
168150
}
169151

170152
else if ((memcmp(_rx_buffer + 3, "GGA,", 3) == 0) && (uiCalcComma == 14) && !_got_pashr_pos_message) {
@@ -262,7 +244,7 @@ int GPSDriverAshtech::handleMessage(int len)
262244
_gps_position->fix_type = 3 + fix_quality - 1;
263245
}
264246

265-
_gps_position->timestamp = gps_absolute_time();
247+
_gps_position->timestamp_sample = gps_absolute_time();
266248

267249
_gps_position->vel_m_s = 0; /**< GPS ground speed (m/s) */
268250
_gps_position->vel_n_m_s = 0; /**< GPS ground speed in m/s */
@@ -444,7 +426,7 @@ int GPSDriverAshtech::handleMessage(int len)
444426
}
445427
}
446428

447-
_gps_position->timestamp = gps_absolute_time();
429+
_gps_position->timestamp_sample = gps_absolute_time();
448430

449431
float track_rad = static_cast<float>(track_true) * M_PI_F / 180.0f;
450432

@@ -705,11 +687,6 @@ int GPSDriverAshtech::handleMessage(int len)
705687

706688
}
707689

708-
if (ret == 1) {
709-
_gps_position->timestamp_time_relative = (int32_t)(_last_timestamp_time - _gps_position->timestamp);
710-
}
711-
712-
713690
// handle survey-in status update
714691
if (_survey_in_start != 0) {
715692
const gps_abstime now = gps_absolute_time();

src/ashtech.h

-1
Original file line numberDiff line numberDiff line change
@@ -128,7 +128,6 @@ class GPSDriverAshtech : public GPSBaseStationSupport
128128

129129
uint8_t _rx_buffer[ASHTECH_RECV_BUFFER_SIZE];
130130
uint16_t _rx_buffer_bytes{};
131-
uint64_t _last_timestamp_time{0};
132131

133132
float _heading_offset;
134133

src/emlid_reach.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -282,7 +282,7 @@ GPSDriverEmlidReach::handleErbSentence()
282282

283283
} else if (_erb_buff.header.id == ERB_ID_GEODIC_POSITION) {
284284

285-
_gps_position->timestamp = gps_absolute_time();
285+
_gps_position->timestamp_sample = gps_absolute_time();
286286

287287
_last_POS_timeGPS = _erb_buff.payload.geodic_position.timeGPS;
288288
_gps_position->lon = round(_erb_buff.payload.geodic_position.longitude * 1e7);

src/femtomes.cpp

+1-2
Original file line numberDiff line numberDiff line change
@@ -114,7 +114,6 @@ int GPSDriverFemto::handleMessage(int len)
114114
_gps_position->vel_e_m_s = _femto_uav_gps.vel_e_m_s;
115115
_gps_position->vel_d_m_s = _femto_uav_gps.vel_d_m_s;
116116
_gps_position->cog_rad = _femto_uav_gps.cog_rad;
117-
_gps_position->timestamp_time_relative = _femto_uav_gps.timestamp_time_relative;
118117
_gps_position->fix_type = _femto_uav_gps.fix_type;
119118
_gps_position->vel_ned_valid = _femto_uav_gps.vel_ned_valid;
120119
_gps_position->satellites_used = _femto_uav_gps.satellites_used;
@@ -134,7 +133,7 @@ int GPSDriverFemto::handleMessage(int len)
134133
_gps_position->heading = NAN;
135134
}
136135

137-
_gps_position->timestamp = gps_absolute_time();
136+
_gps_position->timestamp_sample = gps_absolute_time();
138137

139138
ret = 1;
140139

src/femtomes.h

-1
Original file line numberDiff line numberDiff line change
@@ -66,7 +66,6 @@ typedef struct {
6666
float vel_e_m_s; /** GPS East velocity, (metres/sec)*/
6767
float vel_d_m_s; /** GPS Down velocity, (metres/sec)*/
6868
float cog_rad; /** Course over ground (NOT heading, but direction of movement), -PI..PI, (radians)*/
69-
int32_t timestamp_time_relative;/** timestamp + timestamp_time_relative = Time of the UTC timestamp since system start, (microseconds)*/
7069
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])*/
7170
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.*/
7271
bool vel_ned_valid; /** True if NED velocity is valid*/

src/gps_helper.h

-13
Original file line numberDiff line numberDiff line change
@@ -93,14 +93,6 @@ enum class GPSCallbackType {
9393
* return: ignored
9494
*/
9595
surveyInStatus,
96-
97-
/**
98-
* can be used to set the current clock accurately
99-
* data1: pointer to a timespec struct
100-
* data2: ignored
101-
* return: ignored
102-
*/
103-
setClock,
10496
};
10597

10698
enum class GPSRestartType {
@@ -271,11 +263,6 @@ class GPSHelper
271263
_callback(GPSCallbackType::gotRTCMMessage, buf, buf_length, _callback_user);
272264
}
273265

274-
void setClock(timespec &t)
275-
{
276-
_callback(GPSCallbackType::setClock, &t, 0, _callback_user);
277-
}
278-
279266
/**
280267
* Convert an ECEF (Earth Centered Earth Fixed) coordinate to LLA WGS84 (Lat, Lon, Alt).
281268
* Ported from: https://stackoverflow.com/a/25428344

src/mtk.cpp

+1-18
Original file line numberDiff line numberDiff line change
@@ -264,34 +264,17 @@ GPSDriverMTK::handleMessage(gps_mtk_packet_t &packet)
264264

265265
timeinfo.tm_isdst = 0;
266266

267-
#ifndef NO_MKTIME
268-
269267
time_t epoch = mktime(&timeinfo);
270268

271269
if (epoch > GPS_EPOCH_SECS) {
272-
// FMUv2+ boards have a hardware RTC, but GPS helps us to configure it
273-
// and control its drift. Since we rely on the HRT for our monotonic
274-
// clock, updating it from time to time is safe.
275-
276-
timespec ts{};
277-
ts.tv_sec = epoch;
278-
ts.tv_nsec = timeinfo_conversion_temp * 1000000ULL;
279-
280-
setClock(ts);
281-
282270
_gps_position->time_utc_usec = static_cast<uint64_t>(epoch) * 1000000ULL;
283271
_gps_position->time_utc_usec += timeinfo_conversion_temp * 1000ULL;
284272

285273
} else {
286274
_gps_position->time_utc_usec = 0;
287275
}
288276

289-
#else
290-
_gps_position->time_utc_usec = 0;
291-
#endif
292-
293-
_gps_position->timestamp = gps_absolute_time();
294-
_gps_position->timestamp_time_relative = 0;
277+
_gps_position->timestamp_sample = gps_absolute_time();
295278

296279
// Position and velocity update always at the same time
297280
_rate_count_vel++;

src/nmea.cpp

+6-45
Original file line numberDiff line numberDiff line change
@@ -142,10 +142,8 @@ int GPSDriverNMEA::handleMessage(int len)
142142
int utc_minute = static_cast<int>((utc_time - utc_hour * 10000) / 100);
143143
double utc_sec = static_cast<double>(utc_time - utc_hour * 10000 - utc_minute * 100);
144144

145-
/*
146-
* convert to unix timestamp
147-
*/
148-
struct tm timeinfo = {};
145+
// convert to unix timestamp
146+
struct tm timeinfo {};
149147
timeinfo.tm_year = year - 1900;
150148
timeinfo.tm_mon = month - 1;
151149
timeinfo.tm_mday = day;
@@ -154,36 +152,20 @@ int GPSDriverNMEA::handleMessage(int len)
154152
timeinfo.tm_sec = int(utc_sec);
155153
timeinfo.tm_isdst = 0;
156154

157-
#ifndef NO_MKTIME
158155
time_t epoch = mktime(&timeinfo);
159156

160157
if (epoch > GPS_EPOCH_SECS) {
161158
uint64_t usecs = static_cast<uint64_t>((utc_sec - static_cast<uint64_t>(utc_sec)) * 1000000);
162159

163-
// FMUv2+ boards have a hardware RTC, but GPS helps us to configure it
164-
// and control its drift. Since we rely on the HRT for our monotonic
165-
// clock, updating it from time to time is safe.
166-
167-
if (!_clock_set) {
168-
timespec ts{};
169-
ts.tv_sec = epoch;
170-
ts.tv_nsec = usecs * 1000;
171-
setClock(ts);
172-
_clock_set = true;
173-
}
174-
175160
_gps_position->time_utc_usec = static_cast<uint64_t>(epoch) * 1000000ULL;
176161
_gps_position->time_utc_usec += usecs;
177162

178163
} else {
179164
_gps_position->time_utc_usec = 0;
180165
}
181166

182-
#else
183-
_gps_position->time_utc_usec = 0;
184-
#endif
185167
_TIME_received = true;
186-
_gps_position->timestamp = gps_absolute_time();
168+
_gps_position->timestamp_sample = gps_absolute_time();
187169

188170
} else if ((memcmp(_rx_buffer + 3, "GGA,", 4) == 0) && (uiCalcComma >= 14)) {
189171
/*
@@ -300,7 +282,7 @@ int GPSDriverNMEA::handleMessage(int len)
300282
_FIX_received = true;
301283

302284
_gps_position->c_variance_rad = 0.1f;
303-
_gps_position->timestamp = gps_absolute_time();
285+
_gps_position->timestamp_sample = gps_absolute_time();
304286

305287
} else if (memcmp(_rx_buffer + 3, "HDT,", 4) == 0 && uiCalcComma == 2) {
306288
/*
@@ -506,13 +488,12 @@ int GPSDriverNMEA::handleMessage(int len)
506488
_gps_position->vel_ned_valid = true; /**< Flag to indicate if NED speed is valid */
507489
_gps_position->c_variance_rad = 0.1f;
508490
_gps_position->s_variance_m_s = 0;
509-
_gps_position->timestamp = gps_absolute_time();
510-
_last_timestamp_time = gps_absolute_time();
491+
_gps_position->timestamp_sample = gps_absolute_time();
511492

512493
/*
513494
* convert to unix timestamp
514495
*/
515-
struct tm timeinfo = {};
496+
struct tm timeinfo {};
516497
timeinfo.tm_year = nmea_year + 100;
517498
timeinfo.tm_mon = nmea_mth - 1;
518499
timeinfo.tm_mday = nmea_day;
@@ -521,35 +502,17 @@ int GPSDriverNMEA::handleMessage(int len)
521502
timeinfo.tm_sec = int(utc_sec);
522503
timeinfo.tm_isdst = 0;
523504

524-
#ifndef NO_MKTIME
525505
time_t epoch = mktime(&timeinfo);
526506

527507
if (epoch > GPS_EPOCH_SECS) {
528508
uint64_t usecs = static_cast<uint64_t>((utc_sec - static_cast<uint64_t>(utc_sec)) * 1000000);
529-
530-
// FMUv2+ boards have a hardware RTC, but GPS helps us to configure it
531-
// and control its drift. Since we rely on the HRT for our monotonic
532-
// clock, updating it from time to time is safe.
533-
if (!_clock_set) {
534-
timespec ts{};
535-
ts.tv_sec = epoch;
536-
ts.tv_nsec = usecs * 1000;
537-
538-
setClock(ts);
539-
_clock_set = true;
540-
}
541-
542509
_gps_position->time_utc_usec = static_cast<uint64_t>(epoch) * 1000000ULL;
543510
_gps_position->time_utc_usec += usecs;
544511

545512
} else {
546513
_gps_position->time_utc_usec = 0;
547514
}
548515

549-
#else
550-
_gps_position->time_utc_usec = 0;
551-
#endif
552-
553516
if (!_POS_received && (_last_POS_timeUTC < utc_time)) {
554517
_last_POS_timeUTC = utc_time;
555518
_POS_received = true;
@@ -859,8 +822,6 @@ int GPSDriverNMEA::handleMessage(int len)
859822

860823
if (_VEL_received && _POS_received) {
861824
ret = 1;
862-
_gps_position->timestamp_time_relative = (int32_t)(_last_timestamp_time - _gps_position->timestamp);
863-
_clock_set = false;
864825
_VEL_received = false;
865826
_POS_received = false;
866827
_rate_count_vel++;

src/nmea.h

+1-4
Original file line numberDiff line numberDiff line change
@@ -90,7 +90,6 @@ class GPSDriverNMEA : public GPSHelper
9090
double _last_POS_timeUTC{0};
9191
double _last_VEL_timeUTC{0};
9292
double _last_FIX_timeUTC{0};
93-
uint64_t _last_timestamp_time{0};
9493

9594
uint8_t _sat_num_gga{0};
9695
uint8_t _sat_num_gns{0};
@@ -101,9 +100,7 @@ class GPSDriverNMEA : public GPSHelper
101100
uint8_t _sat_num_gbgsv{0};
102101
uint8_t _sat_num_bdgsv{0};
103102

104-
bool _clock_set {false};
105-
106-
// check if we got all basic essential packages we need
103+
// check if we got all basic essential packages we need
107104
bool _TIME_received{false};
108105
bool _POS_received{false};
109106
bool _ALT_received{false};

src/sbf.cpp

+4-23
Original file line numberDiff line numberDiff line change
@@ -387,8 +387,6 @@ int // 0 = no message handled, 1 = message handled, 2 = sat info message hand
387387
GPSDriverSBF::payloadRxDone()
388388
{
389389
int ret = 0;
390-
struct tm timeinfo;
391-
time_t epoch;
392390

393391
if (_buf.length <= 4 || _buf.crc16 != crc16(reinterpret_cast<uint8_t *>(&_buf) + 4, _buf.length - 4)) {
394392
return 1;
@@ -474,37 +472,24 @@ GPSDriverSBF::payloadRxDone()
474472
}
475473

476474
_gps_position->time_utc_usec = 0;
477-
#ifndef NO_MKTIME
478-
/* convert to unix timestamp */
479-
memset(&timeinfo, 0, sizeof(timeinfo));
480475

476+
/* convert to unix timestamp */
477+
struct tm timeinfo {};
481478
timeinfo.tm_year = 1980 - 1900;
482479
timeinfo.tm_mon = 0;
483480
timeinfo.tm_mday = 6 + _buf.WNc * 7;
484481
timeinfo.tm_hour = 0;
485482
timeinfo.tm_min = 0;
486483
timeinfo.tm_sec = _buf.TOW / 1000;
487484

488-
epoch = mktime(&timeinfo);
485+
time_t epoch = mktime(&timeinfo);
489486

490487
if (epoch > GPS_EPOCH_SECS) {
491-
// FMUv2+ boards have a hardware RTC, but GPS helps us to configure it
492-
// and control its drift. Since we rely on the HRT for our monotonic
493-
// clock, updating it from time to time is safe.
494-
495-
timespec ts;
496-
memset(&ts, 0, sizeof(ts));
497-
ts.tv_sec = epoch;
498-
ts.tv_nsec = (_buf.TOW % 1000) * 1000 * 1000;
499-
setClock(ts);
500-
501488
_gps_position->time_utc_usec = static_cast<uint64_t>(epoch) * 1000000ULL;
502489
_gps_position->time_utc_usec += (_buf.TOW % 1000) * 1000;
503490
}
504491

505-
#endif
506-
_gps_position->timestamp = gps_absolute_time();
507-
_last_timestamp_time = _gps_position->timestamp;
492+
_gps_position->timestamp_sample = gps_absolute_time();
508493
_rate_count_vel++;
509494
_rate_count_lat_lon++;
510495
ret |= (_msg_status == 7) ? 1 : 0;
@@ -536,10 +521,6 @@ GPSDriverSBF::payloadRxDone()
536521
break;
537522
}
538523

539-
if (ret > 0) {
540-
_gps_position->timestamp_time_relative = static_cast<int32_t>(_last_timestamp_time - _gps_position->timestamp);
541-
}
542-
543524
if (ret == 1) {
544525
_msg_status &= ~1;
545526
}

src/sbf.h

-1
Original file line numberDiff line numberDiff line change
@@ -356,7 +356,6 @@ class GPSDriverSBF : public GPSBaseStationSupport
356356
sensor_gps_s *_gps_position { nullptr };
357357
satellite_info_s *_satellite_info { nullptr };
358358
uint8_t _dynamic_model{ 7 };
359-
uint64_t _last_timestamp_time { 0 };
360359
bool _configured { false };
361360
uint8_t _msg_status { 0 };
362361
sbf_decode_state_t _decode_state { SBF_DECODE_SYNC1 };

0 commit comments

Comments
 (0)