From 2ce38b477be385fb126f755ab0df7a957887e363 Mon Sep 17 00:00:00 2001
From: lm <mail@qgroundcontrol.org>
Date: Sun, 4 Sep 2011 11:42:25 +0200
Subject: [PATCH] Fixed wrong message timings in simulated waypoint planner

---
 src/comm/MAVLinkSimulationLink.cc            | 18 ++++---------
 src/comm/MAVLinkSimulationMAV.cc             | 28 ++++++++++----------
 src/comm/MAVLinkSimulationWaypointPlanner.cc |  6 ++---
 3 files changed, 22 insertions(+), 30 deletions(-)

diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc
index 50e87e9ff..44f338b89 100644
--- a/src/comm/MAVLinkSimulationLink.cc
+++ b/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;
diff --git a/src/comm/MAVLinkSimulationMAV.cc b/src/comm/MAVLinkSimulationMAV.cc
index 356104601..ea03225e9 100644
--- a/src/comm/MAVLinkSimulationMAV.cc
+++ b/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),
diff --git a/src/comm/MAVLinkSimulationWaypointPlanner.cc b/src/comm/MAVLinkSimulationWaypointPlanner.cc
index d78b4d5bf..55b876255 100644
--- a/src/comm/MAVLinkSimulationWaypointPlanner.cc
+++ b/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;
-- 
2.22.0