aboutsummaryrefslogtreecommitdiff
path: root/src/drivers/gps
diff options
context:
space:
mode:
Diffstat (limited to 'src/drivers/gps')
-rw-r--r--src/drivers/gps/ashtech.cpp28
-rw-r--r--src/drivers/gps/gps.cpp8
-rw-r--r--src/drivers/gps/gps_helper.cpp11
-rw-r--r--src/drivers/gps/gps_helper.h2
-rw-r--r--src/drivers/gps/mtk.cpp26
-rw-r--r--src/drivers/gps/ubx.cpp58
6 files changed, 82 insertions, 51 deletions
diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp
index a2e292de2..a50767c22 100644
--- a/src/drivers/gps/ashtech.cpp
+++ b/src/drivers/gps/ashtech.cpp
@@ -38,7 +38,7 @@ ASHTECH::~ASHTECH()
int ASHTECH::handle_message(int len)
{
char * endp;
-
+
if (len < 7) { return 0; }
int uiCalcComma = 0;
@@ -99,8 +99,26 @@ int ASHTECH::handle_message(int len)
timeinfo.tm_sec = int(ashtech_sec);
time_t epoch = mktime(&timeinfo);
- _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
- _gps_position->time_gps_usec += (uint64_t)((ashtech_sec - int(ashtech_sec)) * 1e6);
+ if (epoch > GPS_EPOCH_SECS) {
+ uint64_t usecs = static_cast<uint64_t>((ashtech_sec - static_cast<uint64_t>(ashtech_sec))) * 1e6;
+
+ // 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 (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;
+ }
+
_gps_position->timestamp_time = hrt_absolute_time();
}
@@ -611,8 +629,8 @@ void ASHTECH::decode_init(void)
}
-/*
- * ashtech board configuration script
+/*
+ * ashtech board configuration script
*/
const char comm[] = "$PASHS,POP,20\r\n"\
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 47c907bd3..8d7176791 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -552,7 +552,7 @@ start(const char *uart_path, bool fake_gps, bool enable_sat_info)
fd = open(GPS_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
- errx(1, "Could not open device path: %s\n", GPS_DEVICE_PATH);
+ errx(1, "open: %s\n", GPS_DEVICE_PATH);
goto fail;
}
@@ -565,7 +565,7 @@ fail:
g_dev = nullptr;
}
- errx(1, "driver start failed");
+ errx(1, "start failed");
}
/**
@@ -604,7 +604,7 @@ reset()
err(1, "failed ");
if (ioctl(fd, SENSORIOCRESET, 0) < 0)
- err(1, "driver reset failed");
+ err(1, "reset failed");
exit(0);
}
@@ -616,7 +616,7 @@ void
info()
{
if (g_dev == nullptr)
- errx(1, "driver not running");
+ errx(1, "not running");
g_dev->print_info();
diff --git a/src/drivers/gps/gps_helper.cpp b/src/drivers/gps/gps_helper.cpp
index 3b92f1bf4..05dfc99b7 100644
--- a/src/drivers/gps/gps_helper.cpp
+++ b/src/drivers/gps/gps_helper.cpp
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -91,7 +91,7 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud)
warnx("try baudrate: %d\n", speed);
default:
- warnx("ERROR: Unsupported baudrate: %d\n", baud);
+ warnx("ERR: baudrate: %d\n", baud);
return -EINVAL;
}
@@ -109,20 +109,19 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud)
/* set baud rate */
if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
- warnx("ERROR setting config: %d (cfsetispeed)\n", termios_state);
+ warnx("ERR: %d (cfsetispeed)\n", termios_state);
return -1;
}
if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
- warnx("ERROR setting config: %d (cfsetospeed)\n", termios_state);
+ warnx("ERR: %d (cfsetospeed)\n", termios_state);
return -1;
}
if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
- warnx("ERROR setting baudrate (tcsetattr)\n");
+ warnx("ERR: %d (tcsetattr)\n", termios_state);
return -1;
}
- /* XXX if resetting the parser here, ensure it does exist (check for null pointer) */
return 0;
}
diff --git a/src/drivers/gps/gps_helper.h b/src/drivers/gps/gps_helper.h
index 3623397b2..0ce05fcb5 100644
--- a/src/drivers/gps/gps_helper.h
+++ b/src/drivers/gps/gps_helper.h
@@ -43,6 +43,8 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_gps_position.h>
+#define GPS_EPOCH_SECS 1234567890ULL
+
class GPS_Helper
{
public:
diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp
index c4f4f7bec..c0c47073b 100644
--- a/src/drivers/gps/mtk.cpp
+++ b/src/drivers/gps/mtk.cpp
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -73,39 +73,38 @@ MTK::configure(unsigned &baudrate)
/* Write config messages, don't wait for an answer */
if (strlen(MTK_OUTPUT_5HZ) != write(_fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) {
- warnx("mtk: config write failed");
- return -1;
+ goto errout;
}
usleep(10000);
if (strlen(MTK_SET_BINARY) != write(_fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) {
- warnx("mtk: config write failed");
- return -1;
+ goto errout;
}
usleep(10000);
if (strlen(SBAS_ON) != write(_fd, SBAS_ON, strlen(SBAS_ON))) {
- warnx("mtk: config write failed");
- return -1;
+ goto errout;
}
usleep(10000);
if (strlen(WAAS_ON) != write(_fd, WAAS_ON, strlen(WAAS_ON))) {
- warnx("mtk: config write failed");
- return -1;
+ goto errout;
}
usleep(10000);
if (strlen(MTK_NAVTHRES_OFF) != write(_fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) {
- warnx("mtk: config write failed");
- return -1;
+ goto errout;
}
return 0;
+
+errout:
+ warnx("mtk: config write failed");
+ return -1;
}
int
@@ -222,7 +221,6 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
ret = 1;
} else {
- warnx("MTK Checksum invalid");
ret = -1;
}
@@ -282,8 +280,8 @@ MTK::handle_message(gps_mtk_packet_t &packet)
timeinfo_conversion_temp -= timeinfo.tm_sec * 1e3;
time_t epoch = mktime(&timeinfo);
- _gps_position->time_gps_usec = epoch * 1e6; //TODO: test this
- _gps_position->time_gps_usec += timeinfo_conversion_temp * 1e3;
+ _gps_position->time_utc_usec = epoch * 1e6; //TODO: test this
+ _gps_position->time_utc_usec += timeinfo_conversion_temp * 1e3;
_gps_position->timestamp_position = _gps_position->timestamp_time = hrt_absolute_time();
// Position and velocity update always at the same time
diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp
index b0eb4ab66..e197059db 100644
--- a/src/drivers/gps/ubx.cpp
+++ b/src/drivers/gps/ubx.cpp
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (c) 2012, 2013, 2014 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -748,17 +748,23 @@ UBX::payload_rx_done(void)
timeinfo.tm_sec = _buf.payload_rx_nav_pvt.sec;
time_t epoch = mktime(&timeinfo);
-#ifndef CONFIG_RTC
- //Since we lack a hardware RTC, set the system time clock based on GPS UTC
- //TODO generalize this by moving into gps.cpp?
- timespec ts;
- ts.tv_sec = epoch;
- ts.tv_nsec = _buf.payload_rx_nav_pvt.nano;
- clock_settime(CLOCK_REALTIME, &ts);
-#endif
+ 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;
+ if (clock_settime(CLOCK_REALTIME, &ts)) {
+ warn("failed setting clock");
+ }
- _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
- _gps_position->time_gps_usec += (uint64_t)(_buf.payload_rx_nav_pvt.nano * 1e-3f);
+ _gps_position->time_utc_usec = static_cast<uint64_t>(epoch) * 1000000ULL;
+ _gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000;
+ } else {
+ _gps_position->time_utc_usec = 0;
+ }
}
_gps_position->timestamp_time = hrt_absolute_time();
@@ -808,7 +814,7 @@ UBX::payload_rx_done(void)
UBX_TRACE_RXMSG("Rx NAV-TIMEUTC\n");
{
- /* convert to unix timestamp */
+ // convert to unix timestamp
struct tm timeinfo;
timeinfo.tm_year = _buf.payload_rx_nav_timeutc.year - 1900;
timeinfo.tm_mon = _buf.payload_rx_nav_timeutc.month - 1;
@@ -818,17 +824,25 @@ UBX::payload_rx_done(void)
timeinfo.tm_sec = _buf.payload_rx_nav_timeutc.sec;
time_t epoch = mktime(&timeinfo);
-#ifndef CONFIG_RTC
- //Since we lack a hardware RTC, set the system time clock based on GPS UTC
- //TODO generalize this by moving into gps.cpp?
- timespec ts;
- ts.tv_sec = epoch;
- ts.tv_nsec = _buf.payload_rx_nav_timeutc.nano;
- clock_settime(CLOCK_REALTIME, &ts);
-#endif
+ // 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;
+ if (clock_settime(CLOCK_REALTIME, &ts)) {
+ warn("failed setting clock");
+ }
- _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
- _gps_position->time_gps_usec += (uint64_t)(_buf.payload_rx_nav_timeutc.nano * 1e-3f);
+ _gps_position->time_utc_usec = static_cast<uint64_t>(epoch) * 1000000ULL;
+ _gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000;
+ } else {
+ _gps_position->time_utc_usec = 0;
+ }
}
_gps_position->timestamp_time = hrt_absolute_time();
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-18 16:39:33 +0000