Skip to content

Commit

Permalink
remove setClock() callback and add timestamp_sample
Browse files Browse the repository at this point in the history
  • Loading branch information
@dagar
dagar committed Sep 17, 2021
1 parent 6fcf068 commit f7dab1e
Show file tree
Hide file tree
Showing 13 changed files with 40 additions and 189 deletions.
27 changes: 2 additions & 25 deletions src/ashtech.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<uint64_t>((ashtech_sec - static_cast<uint64_t>(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<uint64_t>(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) {
Expand Down Expand Up @@ -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 */
Expand Down Expand Up @@ -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<float>(track_true) * M_PI_F / 180.0f;

Expand Down Expand Up @@ -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();
Expand Down
1 change: 0 additions & 1 deletion src/ashtech.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down
2 changes: 1 addition & 1 deletion src/emlid_reach.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
3 changes: 1 addition & 2 deletions src/femtomes.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;

Expand Down
1 change: 0 additions & 1 deletion src/femtomes.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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*/
Expand Down
13 changes: 0 additions & 13 deletions src/gps_helper.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down Expand Up @@ -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
Expand Down
19 changes: 1 addition & 18 deletions src/mtk.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -264,34 +264,17 @@ 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<uint64_t>(epoch) * 1000000ULL;
_gps_position->time_utc_usec += timeinfo_conversion_temp * 1000ULL;

} else {
_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++;
Expand Down
51 changes: 6 additions & 45 deletions src/nmea.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -142,10 +142,8 @@ int GPSDriverNMEA::handleMessage(int len)
int utc_minute = static_cast<int>((utc_time - utc_hour * 10000) / 100);
double utc_sec = static_cast<double>(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;
Expand All @@ -154,36 +152,20 @@ 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<uint64_t>((utc_sec - static_cast<uint64_t>(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<uint64_t>(epoch) * 1000000ULL;
_gps_position->time_utc_usec += usecs;

} else {
_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)) {
/*
Expand Down Expand Up @@ -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) {
/*
Expand Down Expand Up @@ -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;
Expand All @@ -521,35 +502,17 @@ 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<uint64_t>((utc_sec - static_cast<uint64_t>(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<uint64_t>(epoch) * 1000000ULL;
_gps_position->time_utc_usec += usecs;

} else {
_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;
Expand Down Expand Up @@ -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++;
Expand Down
5 changes: 1 addition & 4 deletions src/nmea.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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};
Expand All @@ -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};
Expand Down
27 changes: 4 additions & 23 deletions src/sbf.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<uint8_t *>(&_buf) + 4, _buf.length - 4)) {
return 1;
Expand Down Expand Up @@ -474,37 +472,24 @@ 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;
timeinfo.tm_hour = 0;
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<uint64_t>(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;
Expand Down Expand Up @@ -536,10 +521,6 @@ GPSDriverSBF::payloadRxDone()
break;
}

if (ret > 0) {
_gps_position->timestamp_time_relative = static_cast<int32_t>(_last_timestamp_time - _gps_position->timestamp);
}

if (ret == 1) {
_msg_status &= ~1;
}
Expand Down
Loading

0 comments on commit f7dab1e

Please sign in to comment.