From 098b57534d338600470531893edfc95e783d1830 Mon Sep 17 00:00:00 2001
From: Pavel Kirienko <pavel.kirienko@gmail.com>
Date: Wed, 5 Apr 2017 16:33:44 +0300
Subject: [PATCH] UAVCAN GNSS bridge: proper handling of various time bases

---
 src/modules/uavcan/sensors/gnss.cpp | 28 +++++++++++++++++++++++++++-
 1 file changed, 27 insertions(+), 1 deletion(-)

diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp
index e6802ec2ac..11ad01e8c1 100644
--- a/src/modules/uavcan/sensors/gnss.cpp
+++ b/src/modules/uavcan/sensors/gnss.cpp
@@ -319,7 +319,33 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
 	report.vel_ned_valid = true;
 
 	report.timestamp_time_relative = 0;
-	report.time_utc_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec();	// Convert to microseconds
+
+	const std::uint64_t gnss_ts_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec();
+
+	switch (msg.gnss_time_standard) {
+	case FixType::GNSS_TIME_STANDARD_UTC: {
+		report.time_utc_usec = gnss_ts_usec;
+		break;
+	}
+
+	case FixType::GNSS_TIME_STANDARD_GPS: {
+		if (msg.num_leap_seconds > 0) {
+			report.time_utc_usec = gnss_ts_usec - msg.num_leap_seconds + 9;
+		}
+		break;
+	}
+
+	case FixType::GNSS_TIME_STANDARD_TAI: {
+		if (msg.num_leap_seconds > 0) {
+			report.time_utc_usec = gnss_ts_usec - msg.num_leap_seconds - 10;
+		}
+		break;
+	}
+
+	default: {
+		break;
+	}
+	}
 
 	report.satellites_used = msg.sats_used;