Diff: com.cpp

Revision:

13:2fb7b19dcd70

Parent:

12:bf4642578682

Child:

14:d2acb373d81c

--- a/com.cpp	Sat Apr 26 02:38:08 2014 +0000
+++ b/com.cpp	Thu May 22 05:58:40 2014 +0000
@@ -16,8 +16,30 @@
 /*              4 -> Yaw                                                 */
 /*************************************************************************/
 
-#include "mbed.h"
 #include "com.h"
+#ifdef DEBUG_COM
+    Timer timerCom;
+    #define NL "\n\r"
+    #define PRINT(x) xbeeTx.printf(x)   //Serial.print(x)
+    #define PRINTF(x, y) xbeeTx.printf( (x) ,(y) )   //Serial.print(x, y)
+    #define PRINTLN(x) PRINT(x);PRINT(NL)
+    #define PRINTLNF(x, y) PRINTF((x) ,(y));PRINT(NL)
+    #define START timerCom.start(); 
+    #define STOP timerCom.stop()
+    #define RESET timerCom.reset()
+    #define READ timerCom.read_us()
+    #define GET(x) x = READ
+#else
+    #define PRINT(x)
+    #define PRINTF(x, y)
+    #define PRINTLN(x)
+    #define PRINTLNF(x, y) 
+    #define START 
+    #define STOP  
+    #define RESET 
+    #define READ 
+    #define GET(x)
+#endif
 
 
 /*********************** com( PinName, PinName ) *************************/
@@ -26,18 +48,19 @@
 
 com::com( PinName tx, PinName rx , PinName rssipin) : xbeeTx( tx, NC), xbeeRx( NC, rx), rssi(rssipin)
 {
-    index1 = 0;                            // How many bytes are in the buffer.
-    index2 = 0;                            // How many bytes are in the buffer.
-    pindex = 0;                            // How many bytes are in the buffer.
-    rdy2build = false;                       // Xbee is in transparent mode.
-    xbeeTx.baud(BAUDRATE);                    // Setup the serial baud rate.
-    xbeeRx.baud(BAUDRATE);                    // Setup the serial baud rate.
+    index1 = 0;                             // How many bytes are in the buffer.
+    index2 = 0;                             // How many bytes are in the buffer.
+    pindex = 0;                             // How many bytes are in the buffer.
+    rdy2build = false;                      // Xbee is in transparent mode.
+    xbeeTx.baud(BAUDRATE);                  // Setup the serial baud rate.
+    xbeeRx.baud(BAUDRATE);                  // Setup the serial baud rate.
     txBuffer = new queue();
     signalStrength = 0;
     xbeeRx.attach( this, &com::callback );    // Set callback as the interrupt handler. 
-    #ifdef DEBUG
+    #ifdef DEBUG_COM
     xbeeTx.printf("Communication.....Done\n\r");
     #endif
+    START;
 }
 
 /************************* bool isData()  ********************************/
@@ -46,7 +69,6 @@
 
 bool com::isData()
 {
-    
     return rdy2build;
 }
 
@@ -79,13 +101,20 @@
 
 void com::ackCheck()
 {
-    if( !txBuffer->isEmpty() && xbeeTx.writeable())
+    if( !txBuffer->isEmpty())
     {
     __disable_irq();
         short * pkt = txBuffer->pop();
+    __enable_irq();
         write(pkt[0],pkt[1]); //may need to disable interrupt
+        #ifdef DEBUG_COM
+        if(pkt[1] % 5 == 0) 
+        {
+            PRINTLNF("len: %d",txBuffer->queueLength());
+        }
+        #endif
         delete[] pkt;
-    __enable_irq();
+    
     }
 }
 
@@ -106,7 +135,7 @@
     while( xbeeRx.readable() )
     {
         char data = xbeeRx.getc(); 
-        
+//        xbeeRx.putc(data);
         if (isA1)
         {
             if ( data == 255 && index1 > 4 )
@@ -118,6 +147,7 @@
             }
             else if ( index1 < BUFFERSIZE )
             {
+                
                 buffer1[index1++] = data;
             }
         }
@@ -149,7 +179,8 @@
 {
     if (rdy2build)
     {
-        char * commandData = new char[pindex];
+        rdy2build = false;
+        char * commandData = new char[5];
         if (!isA1)
         {
             commandData[4] = buffer1[--pindex];     // Sequence Number.
@@ -167,6 +198,7 @@
             commandData[0] = buffer2[--pindex];     // Command.
         }
         
+//        xbeeTx.printf("Copied: %d %d %d %d %d\n\r",commandData[0],commandData[1],commandData[2],commandData[3],commandData[4]);
           
         if( commandData[0] + commandData[2] == commandData[3] ) // Validate checksum.
         {