Sub: Use GCS unix time

Signed-off-by: Patrick José Pereira <patrickelectric@gmail.com>
ArduPilot · Feb 14, 2018 · c0c5749 · c0c5749
1 parent 414589a
commit c0c5749
Show file tree

Hide file tree

Showing 3 changed files with 25 additions and 0 deletions.
diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp
@@ -1558,8 +1558,15 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
         if ((packet.onboard_control_sensors_enabled & MAV_SENSOR_WATER) && !(packet.onboard_control_sensors_health & MAV_SENSOR_WATER)) {
             sub.leak_detector.set_detect();
         }
+        break;
     }
+
+    case MAVLINK_MSG_ID_SYSTEM_TIME: {
+        mavlink_system_time_t packet;
+        mavlink_msg_system_time_decode(msg, &packet);
+        sub.set_system_time(packet.time_unix_usec);
         break;
+    }
 
     default:
         handle_common_message(msg);

diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h
@@ -524,6 +524,7 @@ class Sub : public AP_HAL::HAL::Callbacks {
     void set_ekf_origin(const Location& loc);
     bool far_from_EKF_origin(const Location& loc);
     void set_system_time_from_GPS();
+    void set_system_time(const uint64_t time_unix);
     void exit_mission();
     bool verify_loiter_unlimited();
     bool verify_loiter_time();

diff --git a/ArduSub/commands.cpp b/ArduSub/commands.cpp
@@ -166,3 +166,20 @@ void Sub::set_system_time_from_GPS()
         Log_Write_Event(DATA_SYSTEM_TIME_SET);
     }
 }
+
+void Sub::set_system_time(const uint64_t time_unix)
+{
+    // exit immediately if system time already set
+    if (ap.system_time_set) {
+        return;
+    }
+
+    // set system clock for log timestamps
+    hal.util->set_system_clock(time_unix);
+
+    // update signing timestamp
+    GCS_MAVLINK::update_signing_timestamp(time_unix);
+
+    ap.system_time_set = false;
+    Log_Write_Event(DATA_SYSTEM_TIME_SET);
+}