From d5e48364a1eddf377981cc14e416b06886755ecc Mon Sep 17 00:00:00 2001
From: deweibel <deweibel@f9c3cf11-9bcb-44bc-f272-b75c42450872>
Date: Tue, 1 Feb 2011 23:50:36 +0000
Subject: [PATCH] Change MediaTek timestamps to millisecond Time of Day Add
 epoch method to return timestamp epoch.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1589 f9c3cf11-9bcb-44bc-f272-b75c42450872
---
 libraries/AP_GPS/AP_GPS_MTK.cpp   | 13 +++++++++--
 libraries/AP_GPS/AP_GPS_MTK16.cpp | 39 ++++++++++++++++++++++++++-----
 libraries/AP_GPS/AP_GPS_MTK16.h   |  4 ++++
 libraries/AP_GPS/AP_GPS_UBLOX.cpp |  2 ++
 libraries/AP_GPS/GPS.h            | 22 ++++++++++++++++-
 5 files changed, 71 insertions(+), 9 deletions(-)

diff --git a/libraries/AP_GPS/AP_GPS_MTK.cpp b/libraries/AP_GPS/AP_GPS_MTK.cpp
index 7b0a63358c..3f1a5cec46 100644
--- a/libraries/AP_GPS/AP_GPS_MTK.cpp
+++ b/libraries/AP_GPS/AP_GPS_MTK.cpp
@@ -30,6 +30,9 @@ AP_GPS_MTK::init(void)
 
 	// set 4Hz update rate
 	_port->print(MTK_OUTPUT_4HZ);
+	
+	// set initial epoch code
+	_epoch = TIME_OF_DAY;
 }
 
 // Process bytes available from the stream
@@ -132,8 +135,14 @@ restart:
 			ground_course	= _swapl(&_buffer.msg.ground_course) / 10000;
 			num_sats		= _buffer.msg.satellites;
 			
-			// XXX docs say this is UTC, but our clients expect msToW
-			time			= _swapl(&_buffer.msg.utc_time);
+			// time from gps is UTC, but convert here to msToD
+			long time_utc	= _swapl(&_buffer.msg.utc_time);
+			long 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;
 		}
diff --git a/libraries/AP_GPS/AP_GPS_MTK16.cpp b/libraries/AP_GPS/AP_GPS_MTK16.cpp
index 00195d847d..47a8b86beb 100644
--- a/libraries/AP_GPS/AP_GPS_MTK16.cpp
+++ b/libraries/AP_GPS/AP_GPS_MTK16.cpp
@@ -31,6 +31,11 @@ AP_GPS_MTK16::init(void)
 
 	// set 4Hz update rate
 	_port->print(MTK_OUTPUT_4HZ);
+	
+	// set initial epoch code
+	_epoch = TIME_OF_DAY;
+	_time_offset = 0;
+	_offset_calculated = false;
 }
 
 // Process bytes available from the stream
@@ -47,9 +52,9 @@ AP_GPS_MTK16::init(void)
 bool
 AP_GPS_MTK16::read(void)
 {
-	uint8_t data;
-	int 	numc;
-	bool	parsed = false;
+	uint8_t 	data;
+	int 		numc;
+	bool		parsed = false;
 
 	numc = _port->available();
 	for (int i = 0; i < numc; i++) {	// Process bytes received
@@ -122,12 +127,34 @@ restart:
 			ground_course	= _buffer.msg.ground_course;
 			num_sats		= _buffer.msg.satellites;
 			hdop			= _buffer.msg.hdop;
-			
-			// XXX docs say this is UTC, but our clients expect msToW
-			time			= _swapl(&_buffer.msg.utc_time);
+			date			= _buffer.msg.utc_date;
+				
+			// time from gps is UTC, but convert here to msToD
+			long time_utc	= _buffer.msg.utc_time;				
+			long 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;
+			
+			/*	Waiting on clarification of MAVLink protocol!
+			if(!_offset_calculated && parsed) {
+				long tempd1 = date;
+				long day 	= tempd1/10000;
+				tempd1 		-= day * 10000;
+				long month	= tempd1/100;
+				long year	= tempd1 - month * 100;
+				_time_offset = _calc_epoch_offset(day, month, year);
+				_epoch = UNIX_EPOCH;
+				_offset_calculated = TRUE;
+			}
+			*/
+			
 		}
 	} 
 	return parsed;
 }
+
diff --git a/libraries/AP_GPS/AP_GPS_MTK16.h b/libraries/AP_GPS/AP_GPS_MTK16.h
index c10a482de9..a220d4c50b 100644
--- a/libraries/AP_GPS/AP_GPS_MTK16.h
+++ b/libraries/AP_GPS/AP_GPS_MTK16.h
@@ -55,6 +55,10 @@ private:
 	// State machine state
 	uint8_t 	_step;
 	uint8_t		_payload_counter;
+	
+	// Time from UNIX Epoch offset
+	long		_time_offset;
+	bool		_offset_calculated;
 
 	// Receive buffer
 	union {
diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp
index 8c2328f06e..81980710a4 100644
--- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp
+++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp
@@ -27,6 +27,8 @@ AP_GPS_UBLOX::init(void)
 	// right reporting configuration.
 
 	_port->flush();
+	
+	_epoch = TIME_OF_WEEK;
 }
 
 // Process bytes available from the stream
diff --git a/libraries/AP_GPS/GPS.h b/libraries/AP_GPS/GPS.h
index cd6aea6037..00d3c09d51 100644
--- a/libraries/AP_GPS/GPS.h
+++ b/libraries/AP_GPS/GPS.h
@@ -43,6 +43,22 @@ public:
 	///
 	GPS_Status		status(void) { 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
@@ -53,7 +69,7 @@ public:
 	virtual void	init(void) = 0;
 
 	// Properties
-	long	time;			///< GPS time (FORMAT TBD)
+	long	time;			///< GPS time (milliseconds from epoch)
 	long	date;			///< GPS date (FORMAT TBD)
 	long	latitude;		///< latitude in degrees * 10,000,000
 	long	longitude;		///< longitude in degrees * 10,000,000
@@ -122,6 +138,9 @@ protected:
 	///       printf vs. the potential benefits
 	///
 	void			_error(const char *msg);
+	
+	/// Time epoch code for the gps in use
+	GPS_Time_Epoch				_epoch;
 
 private:
 
@@ -139,6 +158,7 @@ private:
 
 	/// Our current status
 	GPS_Status					_status;
+	
 };
 
 inline long