Diff: DataGlove.cpp

Revision:

19:e751d075e2e6

Parent:

18:6737bf53ccd1

Child:

22:0cc5b7e55419

--- a/DataGlove.cpp	Mon Dec 08 18:56:42 2014 +0000
+++ b/DataGlove.cpp	Mon Dec 08 23:54:33 2014 +0000
@@ -11,6 +11,7 @@
 	notConnectedCount = 0;
 	gl = new Glove;
 	ReceiveCount = 0;
+    numReceived = 0;
 }
 
 DataGlove::~DataGlove()
@@ -37,29 +38,51 @@
     GloveSocket.SendDataToGlove(SendBuf, 6);
 }
 
+void DataGlove::StopSampling()
+{
+    SendBuf[0] = '$';
+    SendBuf[1] = 0x0B;
+    SendBuf[2] = 0x02;
+    // Streaming quaternion data
+    SendBuf[3] = (SendBuf[0]+SendBuf[1]+SendBuf[2])%256;
+    SendBuf[4] = '#';
+    GloveSocket.SendDataToGlove(SendBuf, 6);
+}
+
 void DataGlove::Receive()
 {
     raw = false;
-    numReceived = GloveSocket.GetDataFromBuffer(Buf, 1);
-    if (Buf[0] == '$') {
+    numReceived += GloveSocket.GetDataFromBuffer(Buf, 1);
+    //if(ReceiveCount++%25 == 0) printf("%d %c\r\n", ReceiveCount, Buf[0]);
+    if (Buf[0] == '$' && numReceived > 0) 
+    {
         bcc = '$';
-        numReceived = GloveSocket.GetDataFromBuffer(Buf, 2);
-        if ((numReceived == 2) && (Buf[0] == 0x0a)) {
-            pkglen = Buf[1];
-            bcc += Buf[0];
+        numReceived += GloveSocket.GetDataFromBuffer((Buf+numReceived), 2);
+        if ((numReceived > 2) && (Buf[1] == 0x0a)) 
+        {
+            pkglen = Buf[2];
             bcc += Buf[1];
-            numReceived = GloveSocket.GetDataFromBuffer(Buf, pkglen);
-            if (numReceived < pkglen) {
-                notConnectedCount++;
+            bcc += Buf[2];
+            numReceived += GloveSocket.GetDataFromBuffer((Buf+numReceived), pkglen - numReceived + 3);
+            if ((numReceived  - 3 )< pkglen) 
+            {
+                if(numReceived == 0) notConnectedCount++;
+                return;
             }
-            for (u = 0; u < pkglen - 2; u++) {
+            for (u = 3; u <= pkglen; u++)
+            {
                 bcc += Buf[u];
             }
-            if ((numReceived == pkglen) && (bcc == Buf[pkglen-2])) {
-                memcpy(buffer, Buf, numReceived);
-                timeOfArrival = (buffer[3] << 24) + (buffer[4] << 16) + (buffer[5] << 8) + buffer[6];
+            if ((numReceived - 3 == pkglen) && (bcc == Buf[pkglen+1])) 
+            {
+                memcpy(buffer, (Buf + 3), numReceived - 3);
+                /*timeOfArrival = (buffer[3] << 24) + (buffer[4] << 16) + (buffer[5] << 8) + buffer[6];
                 id = (buffer[1] << 8) + buffer[2];
-                pkgtype = buffer[0];
+                pkgtype = buffer[0];*/
+                timeOfArrival = (Buf[6] << 24) + (Buf[7] << 16) + (Buf[8] << 8) + Buf[9];
+                id = (Buf[4] << 8) + Buf[5];
+                pkgtype = Buf[3];
+                if(ReceiveCount++%25 == 0) printf("%d %c\r\n", ReceiveCount, Buf[0]);
                 switch (pkgtype){
                     case STREAM_FINGERS_QUATERNION:
                     {
@@ -69,7 +92,6 @@
                     case STREAM_QUATERNION:
                     {
                         ExtractQuat();
-                        printf("%d\r\n", ReceiveCount++);
                         //printf("roll: %f\tpitch: %f\tyaw: %f\r\n", gl->roll, gl->pitch, gl->yaw);
                     }
                     break;
@@ -91,25 +113,30 @@
                     }
                     break;
                 }
+                Buf[0] = 0; //Clear Token so no repeats.
                 notConnectedCount = 0;
+                numReceived = 0;
             }
-            else{
-                if (bcc!=Buf[pkglen-2])
+            else
+            {
+                if (bcc!=Buf[pkglen+1])
                     notConnectedCount++;
             }
         }
     }
     else {
         //printf("couldn't get data %d\r\n", notConnectedCount);
+        // Re-establishing communication in case no data is received for 1s (20 frames per second*1 = 20)
+        if (notConnectedCount > 200) {
+            printf("Connection broke! Trying to re-establish... %d \r\n",numReceived);
+            //GloveSocket.Reconnect();
+            //StreamData(STREAM_QUATERNION);
+            notConnectedCount = 0;
+        }
+        numReceived = 0;
         notConnectedCount++;
     }
-    // Re-establishing communication in case no data is received for 1s (20 frames per second*1 = 20)
-    if (notConnectedCount > 20) {
-        printf("Connection broke! Trying to re-establish...\r\n");
-        GloveSocket.Reconnect();
-        StreamData(STREAM_QUATERNION);
-        notConnectedCount = 0;
-    }
+    
 }
 
 void DataGlove::ExtractFingersQuat()