From fd47e0cbb3b3204136086dffc5096cefba9f511a Mon Sep 17 00:00:00 2001
From: Todd Stellanova <todd@otherlab.com>
Date: Thu, 15 Jun 2017 14:48:42 -0700
Subject: [PATCH] Set system real time clock once from GNSS data.

Fixes #7421

Tested with Pixracer and Zubax GNSS2.0
---
 src/modules/uavcan/sensors/gnss.cpp | 17 +++++++++++++++++
 src/modules/uavcan/sensors/gnss.hpp |  2 ++
 2 files changed, 19 insertions(+)

diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp
index 625c2ac7af..1e5f366db2 100644
--- a/src/modules/uavcan/sensors/gnss.cpp
+++ b/src/modules/uavcan/sensors/gnss.cpp
@@ -346,6 +346,23 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
 	}
 	}
 
+//TODO px4_clock_settime does nothing on the Snapdragon platform
+#ifndef __PX4_QURT
+
+	// If we haven't already done so, set the system clock using GPS data
+	if (valid_pos_cov && !_system_clock_set) {
+		timespec ts;
+		memset(&ts, 0, sizeof(ts));
+		// get the whole microseconds
+		ts.tv_sec = report.time_utc_usec / 1000000ULL;
+		// get the remainder microseconds and convert to nanoseconds
+		ts.tv_nsec = (report.time_utc_usec % 1000000ULL) * 1000;
+		px4_clock_settime(CLOCK_REALTIME, &ts);
+		_system_clock_set = true;
+	}
+
+#endif
+
 	report.satellites_used = msg.sats_used;
 
 	// Using PDOP for HDOP and VDOP
diff --git a/src/modules/uavcan/sensors/gnss.hpp b/src/modules/uavcan/sensors/gnss.hpp
index da5b6ffda8..2018a71266 100644
--- a/src/modules/uavcan/sensors/gnss.hpp
+++ b/src/modules/uavcan/sensors/gnss.hpp
@@ -111,4 +111,6 @@ private:
 	orb_advert_t _report_pub;                ///< uORB pub for gnss position
 
 	int _orb_sub_gnss = -1;                  ///< uORB sub for gnss position, used for bridging uORB --> UAVCAN
+
+	bool _system_clock_set = false; ///< Have we set the system clock at least once from GNSS data?
 };