int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user)
{
GPS *gps = (GPS *)user;
switch (type) {
case GPSCallbackType::readDeviceData: {
int num_read = gps->pollOrRead((uint8_t *)data1, data2, *((int *)data1));
if (num_read > 0 && gps->_dump_from_gps_device_fd >= 0) {
if (write(gps->_dump_from_gps_device_fd, data1, (size_t)num_read) != (size_t)num_read) {
PX4_WARN("gps dump failed");
}
}
return num_read;
}
case GPSCallbackType::writeDeviceData:
if (gps->_dump_to_gps_device_fd >= 0) {
if (write(gps->_dump_to_gps_device_fd, data1, (size_t)data2) != (size_t)data2) {
PX4_WARN("gps dump failed");
}
}
return write(gps->_serial_fd, data1, (size_t)data2);
case GPSCallbackType::setBaudrate:
return gps->setBaudrate(data2);
case GPSCallbackType::gotRTCMMessage:
/* not used */
break;
case GPSCallbackType::surveyInStatus:
/* not used */
break;
case GPSCallbackType::setClock:
px4_clock_settime(CLOCK_REALTIME, (timespec *)data1);
break;
}
return 0;
}
int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user)
{
GPS *gps = (GPS *)user;
switch (type) {
case GPSCallbackType::readDeviceData: {
int num_read = gps->pollOrRead((uint8_t *)data1, data2, *((int *)data1));
if (num_read > 0) {
gps->dumpGpsData((uint8_t *)data1, (size_t)num_read, false);
}
return num_read;
}
case GPSCallbackType::writeDeviceData:
gps->dumpGpsData((uint8_t *)data1, (size_t)data2, true);
return write(gps->_serial_fd, data1, (size_t)data2);
case GPSCallbackType::setBaudrate:
return gps->setBaudrate(data2);
case GPSCallbackType::gotRTCMMessage:
/* not used */
break;
case GPSCallbackType::surveyInStatus:
/* not used */
break;
case GPSCallbackType::setClock:
px4_clock_settime(CLOCK_REALTIME, (timespec *)data1);
break;
}
return 0;
}
int ASHTECH::handle_message(int len)
{
char *endp;
if (len < 7) { return 0; }
int uiCalcComma = 0;
for (int i = 0 ; i < len; i++) {
if (_rx_buffer[i] == ',') { uiCalcComma++; }
}
char *bufptr = (char *)(_rx_buffer + 6);
if ((memcmp(_rx_buffer + 3, "ZDA,", 3) == 0) && (uiCalcComma == 6)) {
/*
UTC day, month, and year, and local time zone offset
An example of the ZDA message string is:
$GPZDA,172809.456,12,07,1996,00,00*45
ZDA message fields
Field Meaning
0 Message ID $GPZDA
1 UTC
2 Day, ranging between 01 and 31
3 Month, ranging between 01 and 12
4 Year
5 Local time zone offset from GMT, ranging from 00 through 13 hours
6 Local time zone offset from GMT, ranging from 00 through 59 minutes
7 The checksum data, always begins with *
Fields 5 and 6 together yield the total offset. For example, if field 5 is -5 and field 6 is +15, local time is 5 hours and 15 minutes earlier than GMT.
*/
double ashtech_time = 0.0;
int day = 0, month = 0, year = 0, local_time_off_hour __attribute__((unused)) = 0,
local_time_off_min __attribute__((unused)) = 0;
if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { day = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { month = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { year = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { local_time_off_hour = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { local_time_off_min = strtol(bufptr, &endp, 10); bufptr = endp; }
int ashtech_hour = static_cast<int>(ashtech_time / 10000);
int ashtech_minute = static_cast<int>((ashtech_time - ashtech_hour * 10000) / 100);
double ashtech_sec = static_cast<float>(ashtech_time - ashtech_hour * 10000 - ashtech_minute * 100);
/*
* convert to unix timestamp
*/
struct tm timeinfo;
timeinfo.tm_year = year - 1900;
timeinfo.tm_mon = month - 1;
timeinfo.tm_mday = day;
timeinfo.tm_hour = ashtech_hour;
timeinfo.tm_min = ashtech_minute;
timeinfo.tm_sec = int(ashtech_sec);
// TODO: this functionality is not available on the Snapdragon yet
#ifndef __PX4_QURT
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;
if (px4_clock_settime(CLOCK_REALTIME, &ts)) {
warn("failed setting clock");
}
_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
_gps_position->timestamp_time = hrt_absolute_time();
}
