diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2015-01-05 10:02:07 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2015-01-05 10:02:07 +0100 |
commit | 16b9f666e790a2b939f7890b39cc6e4cf2552165 (patch) | |
tree | ee936b06d15e75e12b175f6786772781aff32cd8 /src/drivers/gps | |
parent | e16c4ff76e7eff2da88bcbef5d05dd4ba11e7203 (diff) | |
parent | c3ed35f5cc8e2617e61747e904dbaa652d1cc2c8 (diff) | |
download | px4-firmware-16b9f666e790a2b939f7890b39cc6e4cf2552165.tar.gz px4-firmware-16b9f666e790a2b939f7890b39cc6e4cf2552165.tar.bz2 px4-firmware-16b9f666e790a2b939f7890b39cc6e4cf2552165.zip |
Merge remote-tracking branch 'upstream/master' into dev_ros
Conflicts:
src/lib/mathlib/math/Matrix.hpp
src/modules/mc_att_control/mc_att_control_main.cpp
src/modules/uORB/topics/vehicle_status.h
src/platforms/px4_includes.h
Diffstat (limited to 'src/drivers/gps')
-rw-r--r-- | src/drivers/gps/ashtech.cpp | 28 | ||||
-rw-r--r-- | src/drivers/gps/gps.cpp | 8 | ||||
-rw-r--r-- | src/drivers/gps/gps_helper.cpp | 11 | ||||
-rw-r--r-- | src/drivers/gps/gps_helper.h | 2 | ||||
-rw-r--r-- | src/drivers/gps/mtk.cpp | 26 | ||||
-rw-r--r-- | src/drivers/gps/ubx.cpp | 58 |
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(); |