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

@@ -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;

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:

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;
         }

libraries/AP_GPS/AP_GPS_MTK19.cpp

@@ -27,6 +27,8 @@
 #include "AP_GPS_MTK19.h"
 #include <stdint.h>
 
+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;
-
-            // 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;
+            
+            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;
+                }
+            }
+
             parsed                  = true;
 
 #ifdef FAKE_GPS_LOCK_TIME

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 {

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;

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;

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;

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;
+}

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:
 
 

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
     }

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");

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
     }

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"));
         }

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
     }