From 2903ff9f730037d4eb6c7be3d925f4219bf719a2 Mon Sep 17 00:00:00 2001
From: Philip Rosedale <philip@coffeeandpower.com>
Date: Mon, 4 Mar 2013 17:00:30 -0800
Subject: [PATCH] Adding support for transmitter data

---
 interface/src/Hand.cpp | 54 +++++++++++++++++++++++++++++++++---------
 interface/src/Hand.h   |  4 +++-
 interface/src/main.cpp |  7 +++++-
 3 files changed, 52 insertions(+), 13 deletions(-)

diff --git a/interface/src/Hand.cpp b/interface/src/Hand.cpp
index 157f0ee38e..bdbc33512c 100644
--- a/interface/src/Hand.cpp
+++ b/interface/src/Hand.cpp
@@ -7,6 +7,7 @@
 //
 
 #include "Hand.h"
+#include <sys/time.h>
 
 const float PHI = 1.618;
 
@@ -24,12 +25,54 @@ Hand::Hand(glm::vec3 initcolor)
     scale.z = scale.y * 1.0;
 }
 
+void Hand::reset()
+{
+    position.x = DEFAULT_X;
+    position.y = DEFAULT_Y;
+    position.z = DEFAULT_Z;
+    pitch = yaw = roll = 0;
+    pitchRate = yawRate = rollRate = 0;
+    setTarget(position);
+    velocity.x = velocity.y = velocity.z = 0;
+    transmitterPackets = 0;
+}
+
+
 void Hand::addAngularVelocity (float pRate, float yRate, float rRate) {
     pitchRate += pRate;
     yawRate += yRate;
     rollRate += rRate;
 }
 
+void Hand::processTransmitterData(char *packetData, int numBytes) {
+    //  Read a packet from a transmitter app, process the data
+    float accX, accY, accZ,
+        graX, graY, graZ,
+        gyrX, gyrY, gyrZ,
+        linX, linY, linZ,
+        rot1, rot2, rot3, rot4;
+    sscanf((char *)packetData, "tacc %f %f %f gra %f %f %f gyr %f %f %f lin %f %f %f rot %f %f %f %f",
+           &accX, &accY, &accZ,
+           &graX, &graY, &graZ,
+           &gyrX, &gyrY, &gyrZ,
+           &linX, &linY, &linZ,
+           &rot1, &rot2, &rot3, &rot4);
+    
+    if (transmitterPackets++ == 0) {
+            gettimeofday(&transmitterTimer, NULL);
+    }
+    const int TRANSMITTER_COUNT = 100;
+    if (transmitterPackets % TRANSMITTER_COUNT == 0) {
+        // Every 100 packets, record the observed Hz of the transmitter data
+        timeval now;
+        gettimeofday(&now, NULL);
+        double msecsElapsed = diffclock(&transmitterTimer, &now);
+        std::cout << "Transmitter Hz: " << (float)TRANSMITTER_COUNT/(msecsElapsed/1000.0) << "\n";
+        //memcpy(&transmitterTimer, &now, sizeof(timeval));
+        transmitterTimer = now; 
+    }
+}
+
 void Hand::render()
 {
     glPushMatrix();
@@ -44,17 +87,6 @@ void Hand::render()
     glPopMatrix();
 }
 
-void Hand::reset()
-{
-    position.x = DEFAULT_X;
-    position.y = DEFAULT_Y;
-    position.z = DEFAULT_Z;
-    pitch = yaw = roll = 0;
-    pitchRate = yawRate = rollRate = 0;
-    setTarget(position);
-    velocity.x = velocity.y = velocity.z = 0;
-}
-
 void Hand::simulate(float deltaTime)
 {
     const float VNOISE = 0.01;
diff --git a/interface/src/Hand.h b/interface/src/Hand.h
index 01478839ee..3769e1fd2e 100644
--- a/interface/src/Hand.h
+++ b/interface/src/Hand.h
@@ -28,11 +28,13 @@ public:
     glm::vec3 getPos() { return position; };
     void setPos(glm::vec3 p) { position = p; };
     void setTarget(glm::vec3 t) { target = t; };
+    void processTransmitterData(char * packetData, int numBytes);
 private:
     glm::vec3 position, target, velocity, color, scale;
     float pitch, yaw, roll, pitchRate, yawRate, rollRate;
     float noise;
-    
+    timeval transmitterTimer;
+    int transmitterPackets;
 };
 
 
diff --git a/interface/src/main.cpp b/interface/src/main.cpp
index 52e602df6a..6d35793677 100644
--- a/interface/src/main.cpp
+++ b/interface/src/main.cpp
@@ -761,7 +761,12 @@ void *networkReceive(void *args)
             packetcount++;
             bytescount += bytesReceived;
             
-            agentList.processAgentData(&senderAddress, incomingPacket, bytesReceived);
+            if (incomingPacket[0] != 't') {
+                //  Pass everything but transmitter data to the agent list 
+                agentList.processAgentData(&senderAddress, incomingPacket, bytesReceived);
+            } else {
+                myHead.hand->processTransmitterData(incomingPacket, bytesReceived);
+            }
         }
     }