Diff: MGC3130.cpp

Revision:

5:4666b36a738d

Parent:

4:6d639fd23628

Child:

6:b511421e7dc8

--- a/MGC3130.cpp	Fri Oct 09 10:45:44 2015 +0000
+++ b/MGC3130.cpp	Fri Oct 09 11:26:22 2015 +0000
@@ -15,16 +15,10 @@
 
 GestICMsg * MGC3130::readMsg()
 {
-    int count = 0;
     TS_Line.input();
-    while (TS_Line.read() == 0) {
-        wait_us(200);
-        count++;
+    if (TS_Line.read() == 0)
+        return NULL;
 
-        // time out after 20 ms
-        if (count > 100)
-            return NULL;
-    }
     msg.reset();
 
     TS_Line.output();
@@ -68,11 +62,8 @@
 
     _i2c_bus.stop();
 
-    if (readMsg() == NULL)
+    while (!(readMsg() != NULL || msg.getID() == GestICMsg::System_Status))
         setRuntimeParameter(para);
 
-    while (msg.getID() != GestICMsg::System_Status)
-        readMsg();
-
     return msg.get(6) | (msg.get(7) << 8);
 }
\ No newline at end of file