From 721feaf40fc2807c19b18ea6bf5f2a2405b1bef4 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Tue, 13 Feb 2018 23:40:26 -0200
Subject: [PATCH] GCS_MAVLink: use AP_RTC
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

GCS_MAVLINK: Add SYSTEM_TIME handle (Patrick José Pereira)
---
 libraries/GCS_MAVLink/GCS.h          |  2 +-
 libraries/GCS_MAVLink/GCS_Common.cpp | 17 ++++++++++++++++-
 2 files changed, 17 insertions(+), 2 deletions(-)

diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
index 3ba7dea531..9824fe452e 100644
--- a/libraries/GCS_MAVLink/GCS.h
+++ b/libraries/GCS_MAVLink/GCS.h
@@ -296,7 +296,7 @@ protected:
     void handle_param_request_list(mavlink_message_t *msg);
     void handle_param_request_read(mavlink_message_t *msg);
     virtual bool params_ready() const { return true; }
-
+    void handle_system_time_message(const mavlink_message_t *msg);
     void handle_common_rally_message(mavlink_message_t *msg);
     void handle_rally_fetch_point(mavlink_message_t *msg);
     void handle_rally_point(mavlink_message_t *msg);
diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
index 8a17bede87..eb71858cd3 100644
--- a/libraries/GCS_MAVLink/GCS_Common.cpp
+++ b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -976,9 +976,12 @@ GCS_MAVLINK::update(uint32_t max_time_us)
  */
 void GCS_MAVLINK::send_system_time()
 {
+    uint64_t time_unix = 0;
+    AP::rtc().get_utc_usec(time_unix); // may fail, leaving time_unix at 0
+
     mavlink_msg_system_time_send(
         chan,
-        AP::gps().time_epoch_usec(),
+        time_unix,
         AP_HAL::millis());
 }
 
@@ -1950,6 +1953,14 @@ void GCS_MAVLINK::handle_statustext(mavlink_message_t *msg)
 }
 
 
+void GCS_MAVLINK::handle_system_time_message(const mavlink_message_t *msg)
+{
+    mavlink_system_time_t packet;
+    mavlink_msg_system_time_decode(msg, &packet);
+
+    AP::rtc().set_utc_usec(packet.time_unix_usec, AP_RTC::SOURCE_MAVLINK_SYSTEM_TIME);
+}
+
 MAV_RESULT GCS_MAVLINK::handle_command_camera(const mavlink_command_long_t &packet)
 {
     AP_Camera *camera = get_camera();
@@ -2331,6 +2342,10 @@ void GCS_MAVLINK::handle_common_message(mavlink_message_t *msg)
     case MAVLINK_MSG_ID_ATT_POS_MOCAP:
         handle_att_pos_mocap(msg);
         break;
+
+    case MAVLINK_MSG_ID_SYSTEM_TIME:
+        handle_system_time_message(msg);
+        break;
     }
 
 }