From 85e5d747109f463feb5d33dd64cbb5b3f0a26927 Mon Sep 17 00:00:00 2001
From: vassilis <vassilis>
Date: Sat, 8 May 2010 00:21:49 +0000
Subject: [PATCH] OP-4 GCS/Telemetry Flush telemetry queues while connection is
 not yet established (to remove stale data)

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@604 ebee16cc-31ac-478f-84a7-5cbb03baadba
---
 ground/src/plugins/uavtalk/telemetry.cpp      | 17 +++++++++++++++
 ground/src/plugins/uavtalk/telemetry.h        |  4 +++-
 .../src/plugins/uavtalk/telemetrymonitor.cpp  | 21 +++++++++++++------
 3 files changed, 35 insertions(+), 7 deletions(-)

diff --git a/ground/src/plugins/uavtalk/telemetry.cpp b/ground/src/plugins/uavtalk/telemetry.cpp
index 9eeb6e55b..698524dce 100644
--- a/ground/src/plugins/uavtalk/telemetry.cpp
+++ b/ground/src/plugins/uavtalk/telemetry.cpp
@@ -49,6 +49,8 @@ Telemetry::Telemetry(UAVTalk* utalk, UAVObjectManager* objMngr)
     connect(objMngr, SIGNAL(newInstance(UAVObject*)), this, SLOT(newInstance(UAVObject*)));
     // Listen to transaction completions
     connect(utalk, SIGNAL(transactionCompleted(UAVObject*)), this, SLOT(transactionCompleted(UAVObject*)));
+    // Get GCS stats object
+    gcsStatsObj = GCSTelemetryStats::GetInstance(objMngr);
     // Setup transaction timer
     transPending = false;
     transTimer = new QTimer(this);
@@ -296,6 +298,7 @@ void Telemetry::processObjectUpdates(UAVObject* obj, EventMask event, bool allIn
         else
         {
             ++txErrors;
+            obj->emitTransactionCompleted(false);
             qxtLog->warning(tr("Telemetry: priority event queue is full, event lost (%1)").arg(obj->getName()));
         }
     }
@@ -308,6 +311,7 @@ void Telemetry::processObjectUpdates(UAVObject* obj, EventMask event, bool allIn
         else
         {
             ++txErrors;
+            obj->emitTransactionCompleted(false);
         }
     }
 
@@ -345,6 +349,19 @@ void Telemetry::processObjectQueue()
         return;
     }
 
+    // Check if a connection has been established, only process GCSTelemetryStats updates
+    // (used to establish the connection)
+    GCSTelemetryStats::DataFields gcsStats = gcsStatsObj->getData();
+    if ( gcsStats.Status != GCSTelemetryStats::STATUS_CONNECTED )
+    {
+        objQueue.clear();
+        if ( objInfo.obj->getObjID() != GCSTelemetryStats::OBJID )
+        {
+            objInfo.obj->emitTransactionCompleted(false);
+            return;
+        }
+    }
+
     // Setup transaction (skip if unpack event)
     if ( objInfo.event != EV_UNPACKED )
     {
diff --git a/ground/src/plugins/uavtalk/telemetry.h b/ground/src/plugins/uavtalk/telemetry.h
index 7d4509202..7cab2265b 100644
--- a/ground/src/plugins/uavtalk/telemetry.h
+++ b/ground/src/plugins/uavtalk/telemetry.h
@@ -31,6 +31,7 @@
 
 #include "uavtalk.h"
 #include "uavobjects/uavobjectmanager.h"
+#include "uavobjects/gcstelemetrystats.h"
 #include <QMutex>
 #include <QMutexLocker>
 #include <QTimer>
@@ -74,7 +75,7 @@ private slots:
 private:
     // Constants
     static const int REQ_TIMEOUT_MS = 250;
-    static const int MAX_RETRIES = 3;
+    static const int MAX_RETRIES = 2;
     static const int MAX_UPDATE_PERIOD_MS = 1000;
     static const int MIN_UPDATE_PERIOD_MS = 1;
     static const int MAX_QUEUE_SIZE = 20;
@@ -113,6 +114,7 @@ private:
     // Variables
     UAVObjectManager* objMngr;
     UAVTalk* utalk;
+    GCSTelemetryStats* gcsStatsObj;
     QList<ObjectTimeInfo> objList;
     QQueue<ObjectQueueInfo> objQueue;
     QQueue<ObjectQueueInfo> objPriorityQueue;
diff --git a/ground/src/plugins/uavtalk/telemetrymonitor.cpp b/ground/src/plugins/uavtalk/telemetrymonitor.cpp
index 8eef09ee5..c9a2714af 100644
--- a/ground/src/plugins/uavtalk/telemetrymonitor.cpp
+++ b/ground/src/plugins/uavtalk/telemetrymonitor.cpp
@@ -186,12 +186,11 @@ void TelemetryMonitor::processStatsUpdates()
     gcsStats.TxRetries += telStats.txRetries;
 
     // Update connection state
+    int oldStatus = gcsStats.Status;
     if ( gcsStats.Status == GCSTelemetryStats::STATUS_DISCONNECTED )
     {
         // Request connection
         gcsStats.Status = GCSTelemetryStats::STATUS_HANDSHAKEREQ;
-        statsTimer->setInterval(STATS_CONNECT_PERIOD_MS);
-        qxtLog->info("Trying to connect to the autopilot");
     }
     else if ( gcsStats.Status == GCSTelemetryStats::STATUS_HANDSHAKEREQ )
     {
@@ -199,9 +198,6 @@ void TelemetryMonitor::processStatsUpdates()
         if ( flightStats.Status == FlightTelemetryStats::STATUS_HANDSHAKEACK )
         {
             gcsStats.Status = GCSTelemetryStats::STATUS_CONNECTED;
-            statsTimer->setInterval(STATS_UPDATE_PERIOD_MS);
-            qxtLog->info("Connection with the autopilot established");
-            startRetrievingObjects();
         }
     }
     else if ( gcsStats.Status == GCSTelemetryStats::STATUS_CONNECTED )
@@ -210,7 +206,6 @@ void TelemetryMonitor::processStatsUpdates()
         if (flightStats.Status == FlightTelemetryStats::STATUS_DISCONNECTED || telStats.rxBytes == 0)
         {
             gcsStats.Status = GCSTelemetryStats::STATUS_DISCONNECTED;
-            qxtLog->info("Connection with the autopilot lost");
         }
     }
 
@@ -223,5 +218,19 @@ void TelemetryMonitor::processStatsUpdates()
     {
         gcsStatsObj->updated();
     }
+
+    // Act on new connections or disconnections
+    if (gcsStats.Status == GCSTelemetryStats::STATUS_CONNECTED && gcsStats.Status != oldStatus)
+    {
+        statsTimer->setInterval(STATS_UPDATE_PERIOD_MS);
+        qxtLog->info("Connection with the autopilot established");
+        startRetrievingObjects();
+    }
+    if (gcsStats.Status == GCSTelemetryStats::STATUS_DISCONNECTED && gcsStats.Status != oldStatus)
+    {
+        statsTimer->setInterval(STATS_CONNECT_PERIOD_MS);
+        qxtLog->info("Connection with the autopilot lost");
+        qxtLog->info("Trying to connect to the autopilot");
+    }
 }