From 6f5ac1d5531b9e8c28b7820d113ae02154223b53 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 23 Oct 2013 22:13:48 +1100 Subject: [PATCH] AP_GPS: added support for GPS time in week/millisec also adds time_epoch_usec() for MAVLink SYSTEM_TIME --- libraries/AP_GPS/AP_GPS_HIL.cpp | 5 +- libraries/AP_GPS/AP_GPS_HIL.h | 2 +- libraries/AP_GPS/AP_GPS_MTK.cpp | 16 ++--- libraries/AP_GPS/AP_GPS_MTK19.cpp | 37 ++++++---- libraries/AP_GPS/AP_GPS_MTK19.h | 7 +- libraries/AP_GPS/AP_GPS_NMEA.cpp | 6 +- libraries/AP_GPS/AP_GPS_SIRF.cpp | 2 +- libraries/AP_GPS/AP_GPS_UBLOX.cpp | 8 ++- libraries/AP_GPS/GPS.cpp | 69 ++++++++++++++++++- libraries/AP_GPS/GPS.h | 36 ++++------ .../examples/GPS_406_test/GPS_406_test.pde | 4 +- .../examples/GPS_AUTO_test/GPS_AUTO_test.pde | 5 +- .../examples/GPS_MTK_test/GPS_MTK_test.pde | 4 +- .../examples/GPS_NMEA_test/GPS_NMEA_test.pde | 17 ++--- .../GPS_UBLOX_test/GPS_UBLOX_test.pde | 4 +- 15 files changed, 143 insertions(+), 79 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS_HIL.cpp b/libraries/AP_GPS/AP_GPS_HIL.cpp index 7e2567dbad..79c05245e4 100644 --- a/libraries/AP_GPS/AP_GPS_HIL.cpp +++ b/libraries/AP_GPS/AP_GPS_HIL.cpp @@ -39,10 +39,11 @@ bool AP_GPS_HIL::read(void) return result; } -void AP_GPS_HIL::setHIL(uint32_t _time, float _latitude, float _longitude, float _altitude, +void AP_GPS_HIL::setHIL(uint64_t _time_epoch_ms, float _latitude, float _longitude, float _altitude, float _ground_speed, float _ground_course, float _speed_3d, uint8_t _num_sats) { - time = _time; + time_week = _time_epoch_ms / (86400*7*(uint64_t)1000); + time_week_ms = _time_epoch_ms - time_week*(86400*7*(uint64_t)1000); latitude = _latitude*1.0e7f; longitude = _longitude*1.0e7f; altitude_cm = _altitude*1.0e2f; diff --git a/libraries/AP_GPS/AP_GPS_HIL.h b/libraries/AP_GPS/AP_GPS_HIL.h index b51f102a76..b09ea054af 100644 --- a/libraries/AP_GPS/AP_GPS_HIL.h +++ b/libraries/AP_GPS/AP_GPS_HIL.h @@ -45,7 +45,7 @@ public: * @param speed_3d - ground speed in meters/second * @param altitude - altitude in meters */ - virtual void setHIL(uint32_t time, float latitude, float longitude, float altitude, + virtual void setHIL(uint64_t time_epoch_ms, float latitude, float longitude, float altitude, float ground_speed, float ground_course, float speed_3d, uint8_t num_sats); private: diff --git a/libraries/AP_GPS/AP_GPS_MTK.cpp b/libraries/AP_GPS/AP_GPS_MTK.cpp index 4790514fbb..8ad475eae9 100644 --- a/libraries/AP_GPS/AP_GPS_MTK.cpp +++ b/libraries/AP_GPS/AP_GPS_MTK.cpp @@ -50,9 +50,6 @@ AP_GPS_MTK::init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting) // Set Nav Threshold to 0 m/s _port->print(MTK_NAVTHRES_OFF); - - // set initial epoch code - _epoch = TIME_OF_DAY; } // Process bytes available from the stream @@ -162,14 +159,11 @@ restart: ground_course_cd = _swapl(&_buffer.msg.ground_course) / 10000; num_sats = _buffer.msg.satellites; - // time from gps is UTC, but convert here to msToD - int32_t time_utc = _swapl(&_buffer.msg.utc_time); - int32_t temp = (time_utc/10000000); - time_utc -= temp*10000000; - time = temp * 3600000; - temp = (time_utc/100000); - time_utc -= temp*100000; - time += temp * 60000 + time_utc; + if (fix >= GPS::FIX_2D) { + _make_gps_time(0, _swapl(&_buffer.msg.utc_time)*10); + } + // we don't change _last_gps_time as we don't know the + // full date parsed = true; } diff --git a/libraries/AP_GPS/AP_GPS_MTK19.cpp b/libraries/AP_GPS/AP_GPS_MTK19.cpp index 7a051e5d1e..a48af0bb58 100644 --- a/libraries/AP_GPS/AP_GPS_MTK19.cpp +++ b/libraries/AP_GPS/AP_GPS_MTK19.cpp @@ -27,6 +27,8 @@ #include "AP_GPS_MTK19.h" #include +extern const AP_HAL::HAL& hal; + // Public Methods ////////////////////////////////////////////////////////////// void AP_GPS_MTK19::init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting) @@ -49,11 +51,6 @@ AP_GPS_MTK19::init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting) // Set Nav Threshold to 0 m/s _port->print(MTK_NAVTHRES_OFF); - - // set initial epoch code - _epoch = TIME_OF_DAY; - _time_offset = 0; - _offset_calculated = false; } // Process bytes available from the stream @@ -166,16 +163,28 @@ restart: ground_course_cd = _buffer.msg.ground_course; num_sats = _buffer.msg.satellites; hdop = _buffer.msg.hdop; - date = _buffer.msg.utc_date; + + if (fix >= GPS::FIX_2D) { + if (_fix_counter == 0) { + uint32_t bcd_time_ms; + if (_mtk_revision == MTK_GPS_REVISION_V16) { + bcd_time_ms = _buffer.msg.utc_time*10; + } else { + bcd_time_ms = _buffer.msg.utc_time; + } + _make_gps_time(_buffer.msg.utc_date, bcd_time_ms); + _last_gps_time = hal.scheduler->millis(); + } + // the _fix_counter is to reduce the cost of the GPS + // BCD time conversion by only doing it every 10s + // between those times we use the HAL system clock as + // an offset from the last fix + _fix_counter++; + if (_fix_counter == 50) { + _fix_counter = 0; + } + } - // time from gps is UTC, but convert here to msToD - int32_t time_utc = _buffer.msg.utc_time; - int32_t temp = (time_utc/10000000); - time_utc -= temp*10000000; - time = temp * 3600000; - temp = (time_utc/100000); - time_utc -= temp*100000; - time += temp * 60000 + time_utc; parsed = true; #ifdef FAKE_GPS_LOCK_TIME diff --git a/libraries/AP_GPS/AP_GPS_MTK19.h b/libraries/AP_GPS/AP_GPS_MTK19.h index a733216c73..17b35873be 100644 --- a/libraries/AP_GPS/AP_GPS_MTK19.h +++ b/libraries/AP_GPS/AP_GPS_MTK19.h @@ -37,7 +37,8 @@ public: GPS(), _step(0), _payload_counter(0), - _mtk_revision(0) + _mtk_revision(0), + _fix_counter(0) {} virtual void init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE); @@ -80,9 +81,7 @@ private: uint8_t _payload_counter; uint8_t _mtk_revision; - // Time from UNIX Epoch offset - long _time_offset; - bool _offset_calculated; + uint8_t _fix_counter; // Receive buffer union { diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index c794a65a36..a9d4d7f9ed 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp @@ -244,8 +244,8 @@ bool AP_GPS_NMEA::_term_complete() if (_gps_data_good) { switch (_sentence_type) { case _GPS_SENTENCE_GPRMC: - time = _new_time; - date = _new_date; + //time = _new_time; + //date = _new_date; latitude = _new_latitude; longitude = _new_longitude; ground_speed_cm = _new_speed; @@ -254,7 +254,7 @@ bool AP_GPS_NMEA::_term_complete() break; case _GPS_SENTENCE_GPGGA: altitude_cm = _new_altitude; - time = _new_time; + //time = _new_time; latitude = _new_latitude; longitude = _new_longitude; num_sats = _new_satellite_count; diff --git a/libraries/AP_GPS/AP_GPS_SIRF.cpp b/libraries/AP_GPS/AP_GPS_SIRF.cpp index cfbe242ef4..8e9ea40b3e 100644 --- a/libraries/AP_GPS/AP_GPS_SIRF.cpp +++ b/libraries/AP_GPS/AP_GPS_SIRF.cpp @@ -179,7 +179,7 @@ AP_GPS_SIRF::_parse_gps(void) { switch(_msg_id) { case MSG_GEONAV: - time = _swapl(&_buffer.nav.time); + //time = _swapl(&_buffer.nav.time); // parse fix type if (_buffer.nav.fix_invalid) { fix = GPS::FIX_NONE; diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 39592cddba..bf36584d3c 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -54,8 +54,6 @@ AP_GPS_UBLOX::init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting) _port->flush(); - _epoch = TIME_OF_WEEK; - // configure the GPS for the messages we want _configure_gps(); @@ -275,7 +273,6 @@ AP_GPS_UBLOX::_parse_gps(void) switch (_msg_id) { case MSG_POSLLH: Debug("MSG_POSLLH next_fix=%u", next_fix); - time = _buffer.posllh.time; longitude = _buffer.posllh.longitude; latitude = _buffer.posllh.latitude; altitude_cm = _buffer.posllh.altitude_msl / 10; @@ -328,6 +325,11 @@ AP_GPS_UBLOX::_parse_gps(void) } num_sats = _buffer.solution.satellites; hdop = _buffer.solution.position_DOP; + if (next_fix >= GPS::FIX_2D) { + time_week_ms = _buffer.solution.time; + time_week = _buffer.solution.week; + _last_gps_time = hal.scheduler->millis(); + } #if UBLOX_FAKE_3DLOCK next_fix = fix; num_sats = 10; diff --git a/libraries/AP_GPS/GPS.cpp b/libraries/AP_GPS/GPS.cpp index 2b213f34ad..1d3d99f33c 100644 --- a/libraries/AP_GPS/GPS.cpp +++ b/libraries/AP_GPS/GPS.cpp @@ -20,8 +20,8 @@ extern const AP_HAL::HAL& hal; GPS::GPS(void) : // ensure all the inherited fields are zeroed - time(0), - date(0), + time_week_ms(0), + time_week(0), latitude(0), longitude(0), altitude_cm(0), @@ -35,6 +35,7 @@ GPS::GPS(void) : valid_read(false), last_fix_time(0), _have_raw_velocity(false), + _last_gps_time(0), _idleTimer(0), _status(GPS::NO_FIX), _last_ground_speed_cm(0), @@ -112,7 +113,7 @@ GPS::update(void) } void -GPS::setHIL(uint32_t _time, float _latitude, float _longitude, float _altitude, +GPS::setHIL(uint64_t _time_epoch_ms, float _latitude, float _longitude, float _altitude, float _ground_speed, float _ground_course, float _speed_3d, uint8_t _num_sats) { } @@ -221,3 +222,65 @@ int16_t GPS::_swapi(const void *bytes) const return(u.v); } + +/** + current time since the unix epoch in microseconds + + This costs about 60 usec on AVR2560 + */ +uint64_t GPS::time_epoch_usec(void) +{ + if (_last_gps_time == 0) { + return 0; + } + const uint64_t ms_per_week = 7000ULL*86400ULL; + const uint64_t unix_offset = 17000ULL*86400ULL + 52*10*7000ULL*86400ULL - 15000ULL; + uint64_t fix_time_ms = unix_offset + time_week*ms_per_week + time_week_ms; + // add in the milliseconds since the last fix + return (fix_time_ms + (hal.scheduler->millis() - _last_gps_time)) * 1000ULL; +} + + +/** + fill in time_week_ms and time_week from BCD date and time components + assumes MTK19 millisecond form of bcd_time + + This function takes about 340 usec on the AVR2560 + */ +void GPS::_make_gps_time(uint32_t bcd_date, uint32_t bcd_milliseconds) +{ + uint8_t year, mon, day, hour, min, sec; + uint16_t msec; + + year = bcd_date % 100; + mon = (bcd_date / 100) % 100; + day = bcd_date / 10000; + msec = bcd_milliseconds % 1000; + + uint32_t v = bcd_milliseconds; + msec = v % 1000; v /= 1000; + sec = v % 100; v /= 100; + min = v % 100; v /= 100; + hour = v % 100; v /= 100; + + int8_t rmon = mon - 2; + if (0 >= rmon) { + rmon += 12; + year -= 1; + } + + // get time in seconds since unix epoch + uint32_t ret = (year/4) - 15 + 367*rmon/12 + day; + ret += year*365 + 10501; + ret = ret*24 + hour; + ret = ret*60 + min; + ret = ret*60 + sec; + + // convert to time since GPS epoch + ret -= 272764785UL; + + // get GPS week and time + time_week = ret / (7*86400UL); + time_week_ms = (ret % (7*86400UL)) * 1000; + time_week_ms += msec; +} diff --git a/libraries/AP_GPS/GPS.h b/libraries/AP_GPS/GPS.h index abe589ba3d..872a5e3db8 100644 --- a/libraries/AP_GPS/GPS.h +++ b/libraries/AP_GPS/GPS.h @@ -72,24 +72,6 @@ public: return _status; } - /// GPS time epoch codes - /// - enum GPS_Time_Epoch { - TIME_OF_DAY = 0, ///< - TIME_OF_WEEK = 1, ///< Ublox - TIME_OF_YEAR = 2, ///< MTK, NMEA - UNIX_EPOCH = 3 ///< If available - }; ///< SIFR? - - - /// Query GPS time epoch - /// - /// @returns Current GPS time epoch code - /// - GPS_Time_Epoch epoch(void) { - return _epoch; - } - /// Startup initialisation. /// /// This routine performs any one-off initialisation required to set the @@ -100,8 +82,8 @@ public: virtual void init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting engine_setting = GPS_ENGINE_NONE) = 0; // Properties - uint32_t time; ///< GPS time (milliseconds from epoch) - uint32_t date; ///< GPS date (FORMAT TBD) + uint32_t time_week_ms; ///< GPS time (milliseconds from start of GPS week) + uint16_t time_week; ///< GPS week number int32_t latitude; ///< latitude in degrees * 10,000,000 int32_t longitude; ///< longitude in degrees * 10,000,000 int32_t altitude_cm; ///< altitude in cm @@ -123,7 +105,7 @@ public: bool print_errors; ///< deprecated // HIL support - virtual void setHIL(uint32_t time, float latitude, float longitude, float altitude, + virtual void setHIL(uint64_t time_epoch_ms, float latitude, float longitude, float altitude, float ground_speed, float ground_course, float speed_3d, uint8_t num_sats); // components of velocity in 2D, in m/s @@ -157,6 +139,9 @@ public: // the time we last processed a message in milliseconds uint32_t last_message_time_ms(void) { return _idleTimer; } + // return last fix time since the 1/1/1970 in microseconds + uint64_t time_epoch_usec(void); + // return true if the GPS supports raw velocity values @@ -198,9 +183,6 @@ protected: /// void _error(const char *msg); - /// Time epoch code for the gps in use - GPS_Time_Epoch _epoch; - enum GPS_Engine_Setting _nav_setting; void _write_progstr_block(AP_HAL::UARTDriver *_fs, const prog_char *pstr, uint8_t size); @@ -218,6 +200,12 @@ protected: // detected baudrate uint16_t _baudrate; + // the time we got the last GPS timestamp + uint32_t _last_gps_time; + + // return time in seconds since GPS epoch given time components + void _make_gps_time(uint32_t bcd_date, uint32_t bcd_milliseconds); + private: diff --git a/libraries/AP_GPS/examples/GPS_406_test/GPS_406_test.pde b/libraries/AP_GPS/examples/GPS_406_test/GPS_406_test.pde index 7800d6a843..2482c3ed68 100644 --- a/libraries/AP_GPS/examples/GPS_406_test/GPS_406_test.pde +++ b/libraries/AP_GPS/examples/GPS_406_test/GPS_406_test.pde @@ -50,8 +50,10 @@ void loop() hal.console->print(gps.num_sats, BASE_DEC); hal.console->print(" FIX:"); hal.console->print(gps.fix, BASE_DEC); + hal.console->print(" WEEK:"); + hal.console->print(gps.time_week, BASE_DEC); hal.console->print(" TIM:"); - hal.console->print(gps.time, BASE_DEC); + hal.console->print(gps.time_week_ms, BASE_DEC); hal.console->println(); gps.new_data = 0; // We have readed the data } diff --git a/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde b/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde index e8f9d99621..649695ad9d 100644 --- a/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde +++ b/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde @@ -50,12 +50,13 @@ void loop() print_latlon(hal.console,gps->latitude); hal.console->print(" Lon: "); print_latlon(hal.console,gps->longitude); - hal.console->printf(" Alt: %.2fm GSP: %.2fm/s CoG: %d SAT: %d TIM: %lu STATUS: %u\n", + hal.console->printf(" Alt: %.2fm GSP: %.2fm/s CoG: %d SAT: %d TIM: %u/%lu STATUS: %u\n", (float)gps->altitude_cm / 100.0, (float)gps->ground_speed_cm / 100.0, (int)gps->ground_course_cd / 100, gps->num_sats, - gps->time, + gps->time_week, + gps->time_week_ms, gps->status()); } else { hal.console->println("No fix"); diff --git a/libraries/AP_GPS/examples/GPS_MTK_test/GPS_MTK_test.pde b/libraries/AP_GPS/examples/GPS_MTK_test/GPS_MTK_test.pde index b0b584f42c..69a82a76f6 100644 --- a/libraries/AP_GPS/examples/GPS_MTK_test/GPS_MTK_test.pde +++ b/libraries/AP_GPS/examples/GPS_MTK_test/GPS_MTK_test.pde @@ -53,8 +53,10 @@ void loop() hal.console->print(gps.num_sats, BASE_DEC); hal.console->print(" FIX:"); hal.console->print(gps.fix, BASE_DEC); + hal.console->print(" WEEK:"); + hal.console->print(gps.time_week, BASE_DEC); hal.console->print(" TIM:"); - hal.console->print(gps.time, BASE_DEC); + hal.console->print(gps.time_week_ms, BASE_DEC); hal.console->println(); gps.new_data = 0; // We have readed the data } diff --git a/libraries/AP_GPS/examples/GPS_NMEA_test/GPS_NMEA_test.pde b/libraries/AP_GPS/examples/GPS_NMEA_test/GPS_NMEA_test.pde index d4cfe4157d..8e02f90d35 100644 --- a/libraries/AP_GPS/examples/GPS_NMEA_test/GPS_NMEA_test.pde +++ b/libraries/AP_GPS/examples/GPS_NMEA_test/GPS_NMEA_test.pde @@ -61,14 +61,15 @@ void loop() if (gps->fix) { hal.console->printf_P( PSTR("Lat: %.7f Lon: %.7f Alt: %.2fm GSP: %.2fm/s " - "CoG: %d SAT: %d TIM: %lu\r\n"), - (float)gps->latitude / T7, - (float)gps->longitude / T7, - (float)gps->altitude_cm / 100.0, - (float)gps->ground_speed_cm / 100.0, - (int)gps->ground_course_cd / 100, - gps->num_sats, - gps->time); + "CoG: %d SAT: %d TIM: %u/%lu\r\n"), + (float)gps->latitude / T7, + (float)gps->longitude / T7, + (float)gps->altitude_cm / 100.0, + (float)gps->ground_speed_cm / 100.0, + (int)gps->ground_course_cd / 100, + gps->num_sats, + gps->time_week, + gps->time_week_ms); } else { hal.console->println_P(PSTR("No fix")); } diff --git a/libraries/AP_GPS/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde b/libraries/AP_GPS/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde index 6159a4a2bc..eb52e34178 100644 --- a/libraries/AP_GPS/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde +++ b/libraries/AP_GPS/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde @@ -58,8 +58,10 @@ void loop() hal.console->print(gps.num_sats, BASE_DEC); hal.console->print(" FIX:"); hal.console->print(gps.fix, BASE_DEC); + hal.console->print(" WEEK:"); + hal.console->print(gps.time_week, BASE_DEC); hal.console->print(" TIM:"); - hal.console->print(gps.time, BASE_DEC); + hal.console->print(gps.time_week_ms, BASE_DEC); hal.console->println(); gps.new_data = 0; // We have readed the data }