Diff: CommandPacket.cpp

Revision:

0:2ba6a9f316b6

Child:

2:54932809c7b2

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CommandPacket.cpp	Sat Jun 04 12:08:49 2016 +0000
@@ -0,0 +1,87 @@
+#include "CommandPacket.h"
+#include "mbed.h"
+
+#define CHECKSUM_DEBUG
+
+uint8_t generate_checksum(uint8_t *data, uint8_t length, uint8_t offset)
+{
+    uint8_t s = 0;
+    
+    if (data)
+    {
+        // take the sum of all data bytes preceding checksum field
+        for(uint8_t i = 0; i < length; i++)
+        {
+            s += data[i + offset];
+        }
+        
+        // calculate two's complement of the remainder
+        s = 0xff - s + 1;
+    }
+    
+    return s;
+}
+
+uint8_t hexchar_to_uint8(uint8_t ch)
+{
+    uint8_t val = 0;
+
+    if (ch >= '0' && ch <= '9')
+    {
+        val = ch - '0';
+    }
+    else if (ch >= 'A' && ch <= 'F')
+    {
+        val  = ch - 'A';
+        val += 10;
+    }
+    else if (ch >= 'a' && ch <= 'f')
+    {
+        val = ch - 'a';
+        val += 10;
+    }
+
+    return val;
+}
+
+bool CommandPacket::verify()
+{
+    return checksum == generate_checksum(payload, length);
+}
+
+CommandPacket::CommandPacket()
+{
+    // reset internal error number
+    errno = NO_ERROR;
+    
+    // set public variables
+    sflag = '<';
+    command = 0x0;
+    length = 0x0;
+    memset(payload, 0x0, sizeof(payload));
+    checksum = 0x0;
+    eflag = '>';
+}
+
+const char *CommandPacket::getErrorString() const
+{
+    return errorString[errno];
+} 
+
+char *CommandPacket::getErrorCode() const
+{
+    // clear error code
+    memset(errorCode, 0x0, sizeof(errorCode));
+    
+    // push into buffer
+    snprintf(errorCode, sizeof(errorCode), "%c%02x%02x%02x%02x%c",
+        sflag,
+        0xe0 | (errno & 0x0f),
+        0x1,
+        command,
+        0x0,
+        eflag
+    );
+    
+    return errorCode;
+}