Diff: YDwifi/YDwifi_uartmsg.cpp

Revision:

1:c6e5f49dce5f

Parent:

0:61c402886fbb

Child:

2:0ba43344c814

--- a/YDwifi/YDwifi_uartmsg.cpp	Thu Mar 06 11:13:00 2014 +0000
+++ b/YDwifi/YDwifi_uartmsg.cpp	Fri Mar 07 02:13:09 2014 +0000
@@ -78,3 +78,49 @@
     
     return uart_cmd_len;
 }
+
+unsigned int C_YD_UartMsg::getResponsePayload( unsigned int cmd_len, unsigned char *recvdata_p
+                                            , unsigned char *command_id_p,  unsigned char *payload_p )
+{
+    unsigned short payload_len  = 0;
+    unsigned int   response_len = 0;
+    unsigned char *buf = NULL;
+    bool isESC = false;
+    int i;
+    
+    // get payload length
+    payload_len = ( ( (recvdata_p[1] & ~0x80) & 0xff) | ( ( (recvdata_p[2] & ~0xC0) << 7) & 0xff00) );
+
+    // get Command ID
+    *command_id_p = (recvdata_p[3] & ~0x80);
+
+    buf = &recvdata_p[4];
+
+    // get payload data
+    for( i = 0; i < payload_len; i++, buf++ )
+    {
+        if( isESC )
+        {
+            *payload_p = (*buf & ~0x80);
+            payload_p++;
+            response_len++;
+        }
+        else
+        {
+            // Check Escape code
+            if( *buf == UART_CMD_ESC )
+            {
+                isESC = true;
+                continue;
+            }
+            else
+            {
+                *payload_p = *buf;
+                payload_p++;
+                response_len++;
+            }
+        }
+    }
+
+    return response_len;
+}
\ No newline at end of file