From f009177aa75f79a38a094087dfd65613eee8ea52 Mon Sep 17 00:00:00 2001
From: Philip Rosedale <philip@coffeeandpower.com>
Date: Thu, 30 May 2013 16:19:07 -0700
Subject: [PATCH] Added check for lost transmitter

---
 interface/src/Application.cpp |  2 ++
 interface/src/Avatar.cpp      |  1 +
 interface/src/Transmitter.cpp | 25 ++++++++++++++++++++++---
 interface/src/Transmitter.h   |  2 ++
 4 files changed, 27 insertions(+), 3 deletions(-)

diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp
index 1e9e119777..72f5fc1a5c 100644
--- a/interface/src/Application.cpp
+++ b/interface/src/Application.cpp
@@ -877,6 +877,8 @@ void Application::idle() {
             _serialPort.readData(deltaTime);
         }
         
+        //  Update transmitter
+        
         //  Sample hardware, update view frustum if needed, and send avatar data to mixer/agents
         updateAvatar(deltaTime);
 
diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp
index 8f6ac7c991..b2e60641a5 100644
--- a/interface/src/Avatar.cpp
+++ b/interface/src/Avatar.cpp
@@ -279,6 +279,7 @@ void Avatar::simulate(float deltaTime, Transmitter* transmitter) {
 
         //  Add thrusts from Transmitter 
         if (transmitter) {
+            transmitter->checkForLostTransmitter();
             glm::vec3 rotation = transmitter->getEstimatedRotation();
             const float TRANSMITTER_MIN_RATE = 1.f;
             const float TRANSMITTER_MIN_YAW_RATE = 4.f;
diff --git a/interface/src/Transmitter.cpp b/interface/src/Transmitter.cpp
index 5f6def92f7..d44227982f 100644
--- a/interface/src/Transmitter.cpp
+++ b/interface/src/Transmitter.cpp
@@ -20,11 +20,25 @@ Transmitter::Transmitter() :
     _isConnected(false),
     _lastRotationRate(0,0,0),
     _lastAcceleration(0,0,0),
-    _estimatedRotation(0,0,0)
+    _estimatedRotation(0,0,0),
+    _lastReceivedPacket(NULL)
 {
     
 }
 
+void Transmitter::checkForLostTransmitter() {
+    //  If we are in motion, check for loss of transmitter packets
+    if (glm::length(_estimatedRotation) > 0.f) {
+        timeval now;
+        gettimeofday(&now, NULL);
+        const int TIME_TO_ASSUME_LOST_MSECS = 2000;
+        int msecsSinceLast = diffclock(_lastReceivedPacket, &now);
+        if (msecsSinceLast > TIME_TO_ASSUME_LOST_MSECS) {
+            resetLevels();
+            printLog("Transmitter signal lost.\n");
+        }
+    }
+}
 void Transmitter::resetLevels() {
     _lastRotationRate *= 0.f;
     _estimatedRotation *= 0.f; 
@@ -34,6 +48,11 @@ void Transmitter::processIncomingData(unsigned char* packetData, int numBytes) {
     const int PACKET_HEADER_SIZE = 1;                   //  Packet's first byte is 'T'
     const int ROTATION_MARKER_SIZE = 1;                 //  'R' = Rotation (clockwise about x,y,z)
     const int ACCELERATION_MARKER_SIZE = 1;             //  'A' = Acceleration (x,y,z)
+    if (!_lastReceivedPacket) {
+        _lastReceivedPacket = new timeval;
+    }
+    gettimeofday(_lastReceivedPacket, NULL);
+    
     if (numBytes == PACKET_HEADER_SIZE + ROTATION_MARKER_SIZE + ACCELERATION_MARKER_SIZE
             + sizeof(_lastRotationRate) + sizeof(_lastAcceleration)
             + sizeof(_touchState.x) + sizeof(_touchState.y) + sizeof(_touchState.state)) {
@@ -69,12 +88,12 @@ void Transmitter::processIncomingData(unsigned char* packetData, int numBytes) {
         _estimatedRotation.y *= (1.f - DECAY_RATE * DELTA_TIME);
 
         if (!_isConnected) {
-            printf("Transmitter V2 Connected.\n");
+            printLog("Transmitter Connected.\n");
             _isConnected = true;
             _estimatedRotation *= 0.0;
         }
     } else {
-        printf("Transmitter V2 packet read error, %d bytes.\n", numBytes);
+        printLog("Transmitter packet read error, %d bytes.\n", numBytes);
     }
 }
 
diff --git a/interface/src/Transmitter.h b/interface/src/Transmitter.h
index 0db0762ab1..95d80249e8 100644
--- a/interface/src/Transmitter.h
+++ b/interface/src/Transmitter.h
@@ -26,6 +26,7 @@ class Transmitter
 public:
     Transmitter();
     void render();
+    void checkForLostTransmitter();
     void resetLevels();
     void renderLevels(int width, int height);
     bool isConnected() { return _isConnected; };
@@ -41,6 +42,7 @@ private:
     glm::vec3 _lastAcceleration;
     glm::vec3 _estimatedRotation;
     TouchState _touchState;
+    timeval* _lastReceivedPacket;
 
 #endif /* defined(__hifi__Transmitter__) */
 };