Diff: Receive.cpp

Revision:

17:2f728fd13bc0

Parent:

16:cdfcb63b2c4b

--- a/Receive.cpp	Wed Mar 20 00:28:27 2013 +0000
+++ b/Receive.cpp	Thu Mar 21 06:51:31 2013 +0000
@@ -22,11 +22,15 @@
 
 #include "XBee.h"
 
-#define LOCK()          NVIC_DisableIRQ(UARTx_IRQn[_serial.index])
-#define UNLOCK()        NVIC_EnableIRQ(UARTx_IRQn[_serial.index])
+#define LOCK()      NVIC_DisableIRQ(UARTx_IRQn[_serial.index])
+#define UNLOCK()    NVIC_EnableIRQ(UARTx_IRQn[_serial.index])
+#define INDEX(n)    ((n) % BUFSIZE)
+#define SIZE(b, i)  (b[i] << 8 | b[INDEX(i + 1)])
+
 const IRQn_Type UARTx_IRQn[] = {UART0_IRQn, UART1_IRQn, UART2_IRQn, UART3_IRQn};
 
 XBee::FrameType XBee::receive(float timeout) {
+    timer.reset();
     flush();
 
     while (true) {
@@ -43,13 +47,13 @@
             free += (2 + size);
             UNLOCK();
             continue;
-        } else if (timeout <= 0) {
+        } else if (timeout <= timer.read()) {
             UNLOCK();
             return None;
+        } else {
+            UNLOCK();
+            sem.wait((int) ((timeout - timer.read()) * 1000));
         }
-        UNLOCK();
-        wait(0.001);
-        timeout -= 0.001;
     }
 }
 
@@ -60,8 +64,8 @@
  * if the frame is found before timeout, returns the index of the packet, otherwise -1.
  *
  */
-int XBee::seekFor(FrameType type, char id, float timeout)
-{
+int XBee::seekFor(FrameType type, char id, float timeout) {
+    timer.reset();
     LOCK();
 
     while (out != in && getFrameType(buf[INDEX(out + 2)]) == None) {
@@ -77,23 +81,21 @@
                 UNLOCK();
                 return index;
             }
-            //::printf("seekFor: index = %02X\n", index);
             int size = SIZE(buf, index);
             index = INDEX(index + 2 + size);
             continue;
-        } else if (timeout <= 0) {
+        } else if (timeout <= timer.read()) {
             UNLOCK();
             return -1;
+        } else {
+            UNLOCK();
+            sem.wait((int) ((timeout - timer.read()) * 1000));
+            LOCK();
         }
-        UNLOCK();
-        wait(0.001);
-        timeout -= 0.001;
-        LOCK();
     }
 }
 
-XBee::FrameType XBee::getFrameType(char c)
-{
+XBee::FrameType XBee::getFrameType(char c) {
     switch (c) {
         case 0x00:
             return None;
@@ -120,8 +122,7 @@
     }
 }
 
-void XBee::flush()
-{
+void XBee::flush() {
     LOCK();
     if (received == out) {
         do {
@@ -162,7 +163,7 @@
     }
 
     if (c != -1) {
-        if (apiMode == 2) {
+        if (api == 2) {
             if (escaped) {
                 c ^= 0x20;
                 escaped = false;
@@ -209,6 +210,7 @@
                     free = in <= out ? out - in : BUFSIZE + out - in; // maybe in == out, then free == 0, but != BUFSIZE
                 }
                 state = UNKNOWN;
+                sem.release();
                 break;
                 
             default: