Fixed wrong message timings in simulated waypoint planner

src/comm/MAVLinkSimulationLink.cc

@@ -36,7 +36,7 @@ This file is part of the QGROUNDCONTROL project
 #include <QTime>
 #include <QImage>
 #include <QDebug>
-#include "MG.h"
+#include <QFileInfo>
 #include "LinkManager.h"
 #include "MAVLinkProtocol.h"
 #include "MAVLinkSimulationLink.h"
@@ -78,7 +78,7 @@ MAVLinkSimulationLink::MAVLinkSimulationLink(QString readFile, QString writeFile
         simulationHeader = simulationFile->readLine();
     }
     receiveFile = new QFile(writeFile, this);
-    lastSent = MG::TIME::getGroundTimeNow() * 1000;
+    lastSent = QGC::groundTimeMilliseconds();
 
     if (simulationFile->exists()) {
         this->name = "Simulation: " + QFileInfo(simulationFile->fileName()).fileName();
@@ -118,7 +118,7 @@ void MAVLinkSimulationLink::run()
 
         static quint64 last = 0;
 
-        if (MG::TIME::getGroundTimeNow() - last >= rate) {
+        if (QGC::groundTimeMilliseconds() - last >= rate) {
             if (_isConnected) {
                 mainloop();
 
@@ -132,7 +132,7 @@ void MAVLinkSimulationLink::run()
 
                 readBytes();
             }
-            last = MG::TIME::getGroundTimeNow();
+            last = QGC::groundTimeMilliseconds();
         }
         QGC::SLEEP::msleep(3);
 
@@ -265,9 +265,6 @@ void MAVLinkSimulationLink::mainloop()
                     double d = QString(parts.at(i)).toDouble(&res);
                     if (!res) d = 0;
 
-                    //qDebug() << "TIME" << time << "VALUE" << d;
-                    //emit valueChanged(220, keys.at(i), d, MG::TIME::getGroundTimeNow());
-
                     if (keys.value(i, "") == "Accel._X") {
                         rawImuValues.xacc = d;
                     }
@@ -345,7 +342,7 @@ void MAVLinkSimulationLink::mainloop()
 
                 //qDebug() << "ATTITUDE" << "BUF LEN" << bufferlength << "POINTER" << streampointer;
 
-                //qDebug() << "REALTIME" << MG::TIME::getGroundTimeNow() << "ONBOARDTIME" << attitude.msec << "ROLL" << attitude.roll;
+                //qDebug() << "REALTIME" << QGC::groundTimeMilliseconds() << "ONBOARDTIME" << attitude.msec << "ROLL" << attitude.roll;
 
             }
 
@@ -667,11 +664,6 @@ qint64 MAVLinkSimulationLink::bytesAvailable()
 
 void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
 {
-    //qDebug() << "Simulation received " << size << " bytes from groundstation: ";
-
-    // Increase write counter
-    //bitsSentTotal += size * 8;
-
     // Parse bytes
     mavlink_message_t msg;
     mavlink_status_t comm;

src/comm/MAVLinkSimulationMAV.cc

@@ -23,20 +23,20 @@ MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int sy
     yaw(0.0),
     globalNavigation(true),
     firstWP(false),
-//    previousSPX(8.548056),
-//    previousSPY(47.376389),
-//    previousSPZ(550),
-//    previousSPYaw(0.0),
-//    nextSPX(8.548056),
-//    nextSPY(47.376389),
-//    nextSPZ(550),
-        previousSPX(37.480391),
-        previousSPY(122.282883),
-        previousSPZ(550),
-        previousSPYaw(0.0),
-        nextSPX(37.480391),
-        nextSPY(122.282883),
-        nextSPZ(550),
+    //    previousSPX(8.548056),
+    //    previousSPY(47.376389),
+    //    previousSPZ(550),
+    //    previousSPYaw(0.0),
+    //    nextSPX(8.548056),
+    //    nextSPY(47.376389),
+    //    nextSPZ(550),
+    previousSPX(37.480391),
+    previousSPY(122.282883),
+    previousSPZ(550),
+    previousSPYaw(0.0),
+    nextSPX(37.480391),
+    nextSPY(122.282883),
+    nextSPZ(550),
     nextSPYaw(0.0),
     sys_mode(MAV_MODE_READY),
     sys_state(MAV_STATE_STANDBY),

src/comm/MAVLinkSimulationWaypointPlanner.cc

@@ -450,7 +450,7 @@ MAVLinkSimulationWaypointPlanner::MAVLinkSimulationWaypointPlanner(MAVLinkSimula
     timestamp_last_send_setpoint(0),
     systemid(sysid),
     compid(MAV_COMP_ID_WAYPOINTPLANNER),
-    setpointDelay(10),
+    setpointDelay(1000),
     yawTolerance(0.4f),
     verbose(true),
     debug(false),
@@ -553,7 +553,7 @@ void MAVLinkSimulationWaypointPlanner::send_setpoint(uint16_t seq)
             emit messageSent(msg);
         }
 
-        uint64_t now = QGC::groundTimeUsecs()/1000;
+        uint64_t now = QGC::groundTimeMilliseconds();
         timestamp_last_send_setpoint = now;
     }
 }
@@ -699,7 +699,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
 
     //qDebug() << "MAV: %d WAYPOINTPLANNER GOT MESSAGE" << systemid;
 
-    uint64_t now = QGC::groundTimeUsecs()/1000;
+    uint64_t now = QGC::groundTimeMilliseconds();
     if (now-protocol_timestamp_lastaction > protocol_timeout && current_state != PX_WPP_IDLE) {
         if (verbose) qDebug() << "Last operation (state=%u) timed out, changing state to PX_WPP_IDLE" << current_state;
         current_state = PX_WPP_IDLE;