Skip to content
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

Jump to bottom

[WIP] remove setClock() callback and add timestamp_sample #90

Draft
wants to merge 1 commit into
base: main
Choose a base branch
from
Draft

[WIP] remove setClock() callback and add timestamp_sample #90

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
27 changes: 2 additions & 25 deletions src/ashtech.cpp
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
1 change: 0 additions & 1 deletion src/sbf.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 };
Expand Down
Loading