Diff: Hexi_KW40Z.cpp

Revision:

11:a9a838035b87

Parent:

10:1bed2b28ee18

--- a/Hexi_KW40Z.cpp	Mon Sep 26 02:36:12 2016 +0000
+++ b/Hexi_KW40Z.cpp	Mon Sep 26 04:56:27 2016 +0000
@@ -36,7 +36,7 @@
 #include "Hexi_KW40Z.h"
  
 KW40Z::KW40Z(PinName txPin,PinName rxPin) : 
-#if defined (LIB_DEBUG)
+#if defined (LIB_DEBUG) || defined (RAW_DEBUG)
 pc(USBTX, USBRX),
 #endif
 device(txPin, rxPin), rxThread(osPriorityNormal,1024), mainThread(osPriorityNormal,1024)
@@ -207,7 +207,10 @@
 }
 
 void KW40Z::ProcessBuffer()
-{    
+{
+#if defined (RAW_DEBUG)
+    pc.printf("%02X ", rxBuff-1);
+#endif 
     /* check if header has been received */
     if(rxBuff > ((uint8_t*)&hostInterface_rxPacket + gHostInterface_headerSize))
     {
@@ -256,7 +259,10 @@
 #if defined (LIB_DEBUG)
                     pc.printf("RX: ");
                     DebugPrintPacket(&hostInterface_rxPacket);
-#endif 
+#endif
+#if defined (RAW_DEBUG)
+                    pc.printf("\r\n");
+#endif
                     /* reset buffer position */
                     rxBuff = (uint8_t*)&hostInterface_rxPacket;
                 } 
@@ -268,20 +274,21 @@
 void KW40Z::SearchStartByte()
 {
     bool found = false;
-    uint8_t * rdIdx = (uint8_t*)&hostInterface_rxPacket + 1;
-    
+    uint8_t * rdIdx = (uint8_t*)&hostInterface_rxPacket;
+    rdIdx++;
+        
     while(rdIdx < rxBuff)
     {
         if(*rdIdx == gHostInterface_startByte1)
         {
-            uint32_t len = rxBuff - rdIdx;
+            uint32_t len = rxBuff - rdIdx + 1;
             
             memcpy(&hostInterface_rxPacket,rdIdx,len);
-            rxBuff -= len;
+            rxBuff = (uint8_t*)&hostInterface_rxPacket + len;
             found = true;
             
 #if defined (LIB_DEBUG) 
-            pc.printf("moving ");
+            pc.printf("moving %d", len);
 #endif
             break;
         }
@@ -292,10 +299,14 @@
     {
         /* reset buffer position */
         rxBuff = (uint8_t*)&hostInterface_rxPacket;
+        memset(rxBuff, 0, sizeof(hostInterface_packet_t));
     }
     
 #if defined (LIB_DEBUG) 
-    pc.printf("Search done: ");
+    pc.printf("Search done: \r\n");
+    pc.printf("rxBuff: ");
+    DebugPrintPacket((hostInterface_packet_t*)rxBuff);
+    pc.printf("rxPack: ");
     DebugPrintPacket(&hostInterface_rxPacket);
 #endif
 }
@@ -728,9 +739,9 @@
     char * idx = (char *)packet;
     uint8_t length = packet->length + gHostInterface_headerSize + 1;
     
-    if(length > (gHostInterface_dataSize + gHostInterface_headerSize + 1))
+    if(length > sizeof(hostInterface_packet_t))
     {
-        length = gHostInterface_dataSize + gHostInterface_headerSize + 1;
+        length = sizeof(hostInterface_packet_t);
     }
 
     for(uint8_t i = 0; i < length; i++)