Revision 19:e751d075e2e6, committed 2014-12-08

Comitter:

kalbers

Date:

Mon Dec 08 23:54:33 2014 +0000

Parent:

18:6737bf53ccd1

Child:

21:5b987194387d

Commit message:

Put Reconnect error recovering in GloveWifi instead of DataGlove;

Changed in this revision

--- 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()

--- a/DataGlove.h	Mon Dec 08 18:56:42 2014 +0000
+++ b/DataGlove.h	Mon Dec 08 23:54:33 2014 +0000
@@ -27,8 +27,8 @@
 	private:
         uint8_t SendBuf[20];
         int16_t notConnectedCount;
-		int8_t numReceived;
-	    uint8_t Buf[256], buffer[256];
+		long numReceived;
+	    char Buf[256], buffer[256];
 	    bool raw;
 	    unsigned char bcc;
 	    int pkglen, u;
@@ -45,6 +45,7 @@
     private:
 		void Parse();
 		void StreamData(uint8_t DataType);
+		void StopSampling();
 		void ExtractFingersQuat();
 		void ExtractQuat();
 		void ExtractFingersRaw();

--- a/GloveWifi.cpp	Mon Dec 08 18:56:42 2014 +0000
+++ b/GloveWifi.cpp	Mon Dec 08 23:54:33 2014 +0000
@@ -12,7 +12,8 @@
 #define ECHO_SERVER_PORT        2000
 #define MMA8451_I2C_ADDRESS     (0x1d<<1)
 
-GloveWifi::GloveWifi()
+GloveWifi::GloveWifi():
+DropCount(0)
 {
 }
 
@@ -60,8 +61,8 @@
         printf("Unable to connect to (%s) on port (%d) \r\n", ECHO_SERVER_ADDRESS, ECHO_SERVER_PORT);
         wait(1);
     }
-    socket->set_blocking(true);
     printf("Connected!\r\n");
+    socket->set_blocking(false, 10);
     
 }
 
@@ -82,14 +83,29 @@
         printf("Unable to connect to (%s) on port (%d) \r\n", ECHO_SERVER_ADDRESS, ECHO_SERVER_PORT);
         wait(1);
     }
-    socket->set_blocking(true);
+    socket->set_blocking(false, 10);
     printf("Reconnected!\r\n");
 }
 
-uint8_t GloveWifi::GetDataFromBuffer(uint8_t * buf, uint16_t size)
+int GloveWifi::GetDataFromBuffer(char * buf, int size)
 {
     //printf("Buffer Size: %d\r\n", wifi->_simple_link.get_transmit_error());
-    return socket->receive((char *)buf, size);
+    int numR = 0;
+    numR = socket->receive((char *)buf, size);
+    if(numR == -1)
+    {
+        DropCount++;
+    }else
+    {
+        DropCount = 0;
+    }
+    if(DropCount > 200)
+    {
+        printf("!\r\n");
+        Reconnect();
+        DropCount = 0;
+    }
+    return numR;
 }
 
 uint8_t GloveWifi::SendDataToGlove(uint8_t * buf, uint16_t size)

--- a/GloveWifi.h	Mon Dec 08 18:56:42 2014 +0000
+++ b/GloveWifi.h	Mon Dec 08 23:54:33 2014 +0000
@@ -15,11 +15,12 @@
 		void Connect();
 		void Disconnect();
 		void Reconnect();
-		uint8_t GetDataFromBuffer(uint8_t * buf, uint16_t size);
+		int GetDataFromBuffer(char * buf, int size);
 		uint8_t SendDataToGlove(uint8_t * buf, uint16_t size);
 
 	private:
     	TCPSocketConnection * socket;
 		cc3000 * wifi;
+		uint32_t DropCount;
 	
 };
\ No newline at end of file