Merge pull request #6246 from bluerobotics/patrickelectric/set_time

Set vehicle time when finishing initial parameters read

src/AutoPilotPlugins/AutoPilotPlugin.cc

@@ -13,8 +13,6 @@
 
 #include "AutoPilotPlugin.h"
 #include "QGCApplication.h"
-#include "ParameterManager.h"
-#include "UAS.h"
 #include "FirmwarePlugin.h"
 
 AutoPilotPlugin::AutoPilotPlugin(Vehicle* vehicle, QObject* parent)
@@ -28,7 +26,7 @@ AutoPilotPlugin::AutoPilotPlugin(Vehicle* vehicle, QObject* parent)
 
 AutoPilotPlugin::~AutoPilotPlugin()
 {
-    
+
 }
 
 void AutoPilotPlugin::_recalcSetupComplete(void)

src/AutoPilotPlugins/AutoPilotPlugin.h

@@ -19,7 +19,6 @@
 #include "FactSystem.h"
 #include "Vehicle.h"
 
-class ParameterManager;
 class Vehicle;
 class FirmwarePlugin;
 

src/Vehicle/Vehicle.cc

@@ -2255,12 +2255,34 @@ void Vehicle::_rallyPointLoadComplete(void)
 
 void Vehicle::_parametersReady(bool parametersReady)
 {
+    // Try to set current unix time to the vehicle
+    _sendQGCTimeToVehicle();
+    // Send time twice, more likely to get to the vehicle on a noisy link
+    _sendQGCTimeToVehicle();
     if (parametersReady) {
         _setupAutoDisarmSignalling();
         _startPlanRequest();
     }
 }
 
+void Vehicle::_sendQGCTimeToVehicle(void)
+{
+    mavlink_message_t       msg;
+    mavlink_system_time_t   cmd;
+
+    // Timestamp of the master clock in microseconds since UNIX epoch.
+    cmd.time_unix_usec = QDateTime::currentDateTime().currentMSecsSinceEpoch()*1000;
+    // Timestamp of the component clock since boot time in milliseconds (Not necessary).
+    cmd.time_boot_ms = 0;
+    mavlink_msg_system_time_encode_chan(_mavlink->getSystemId(),
+                                    _mavlink->getComponentId(),
+                                    priorityLink()->mavlinkChannel(),
+                                    &msg,
+                                    &cmd);
+
+    sendMessageOnLink(priorityLink(), msg);
+}
+
 void Vehicle::disconnectInactiveVehicle(void)
 {
     // Vehicle is no longer communicating with us, disconnect all links

src/Vehicle/Vehicle.h

@@ -11,7 +11,6 @@
 
 #include <QObject>
 #include <QGeoCoordinate>
-#include <QElapsedTimer>
 
 #include "FactGroup.h"
 #include "LinkInterface.h"
@@ -994,6 +993,7 @@ private slots:
     void _updateDistanceToHome(void);
     void _updateHobbsMeter(void);
     void _vehicleParamLoaded(bool ready);
+    void _sendQGCTimeToVehicle(void);
 
 private:
     bool _containsLink(LinkInterface* link);