diff --git a/ground/src/plugins/uavtalk/telemetrymonitor.cpp b/ground/src/plugins/uavtalk/telemetrymonitor.cpp index c354e32ad..245105f2b 100644 --- a/ground/src/plugins/uavtalk/telemetrymonitor.cpp +++ b/ground/src/plugins/uavtalk/telemetrymonitor.cpp @@ -37,6 +37,7 @@ TelemetryMonitor::TelemetryMonitor(UAVObjectManager* objMngr, Telemetry* tel) this->objMngr = objMngr; this->tel = tel; this->objPending = NULL; + this->connectionTimer = new QTime(); // Create mutex mutex = new QMutex(QMutex::Recursive); @@ -185,6 +186,21 @@ void TelemetryMonitor::processStatsUpdates() gcsStats.TxFailures += telStats.txErrors; gcsStats.TxRetries += telStats.txRetries; + // Check for a connection timeout + bool connectionTimeout; + if ( telStats.rxObjects > 0 ) + { + connectionTimer->start(); + } + if ( connectionTimer->elapsed() > CONNECTION_TIMEOUT_MS ) + { + connectionTimeout = true; + } + else + { + connectionTimeout = false; + } + // Update connection state int oldStatus = gcsStats.Status; if ( gcsStats.Status == GCSTelemetryStats::STATUS_DISCONNECTED ) @@ -203,7 +219,7 @@ void TelemetryMonitor::processStatsUpdates() else if ( gcsStats.Status == GCSTelemetryStats::STATUS_CONNECTED ) { // Check if the connection is still active and the the autopilot is still connected - if (flightStats.Status == FlightTelemetryStats::STATUS_DISCONNECTED || telStats.rxObjects == 0) + if (flightStats.Status == FlightTelemetryStats::STATUS_DISCONNECTED || connectionTimeout) { gcsStats.Status = GCSTelemetryStats::STATUS_DISCONNECTED; } diff --git a/ground/src/plugins/uavtalk/telemetrymonitor.h b/ground/src/plugins/uavtalk/telemetrymonitor.h index 7b804f156..7082389ca 100644 --- a/ground/src/plugins/uavtalk/telemetrymonitor.h +++ b/ground/src/plugins/uavtalk/telemetrymonitor.h @@ -32,6 +32,7 @@ #include #include #include +#include #include #include #include "uavobjects/uavobjectmanager.h" @@ -53,8 +54,9 @@ public slots: void flightStatsUpdated(UAVObject* obj); private: - static const int STATS_UPDATE_PERIOD_MS = 5000; + static const int STATS_UPDATE_PERIOD_MS = 4000; static const int STATS_CONNECT_PERIOD_MS = 1000; + static const int CONNECTION_TIMEOUT_MS = 8000; UAVObjectManager* objMngr; Telemetry* tel; @@ -64,6 +66,7 @@ private: QTimer* statsTimer; UAVObject* objPending; QMutex* mutex; + QTime* connectionTimer; void startRetrievingObjects(); void retrieveNextObject();