Diff: AlohaPacket.cpp

Revision:

16:c3c6b13c3c42

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AlohaPacket.cpp	Wed Jun 08 22:15:29 2016 +0000
@@ -0,0 +1,75 @@
+#include "AlohaPacket.h"
+
+uint8_t crc8(const uint8_t *data, int len)
+{
+    unsigned int crc = 0;
+    
+    for (int j = len; j; j--, data++)
+    {
+        crc ^= (*data << 8);
+        for (int i = 8; i; i--)
+        {
+            if (crc & 0x8000)
+            {
+                crc ^= (0x1070 << 3);
+            }
+            crc <<= 1;
+        }
+    }
+    
+    return (uint8_t)(crc >> 8);
+}
+
+bool dissectAlohaPacket(uint8_t *input, HeaderStruct *header, DataStruct *data)
+{
+    // assume user has already prepare memory in *in
+    
+    // get header
+    if (header)
+    {
+        header->fid = input[0] >> 4; // higher four bits
+        header->no = input[0] & 0x0f; // lower four bits
+    }
+    
+    // get data
+    if (data)
+    {
+        data->pd0 = input[1];
+        data->pd1 = input[2];
+    }
+    
+    // check crc
+    if (header && data)
+    {
+        return input[3] == crc8(input, 3);
+    }
+    else
+    {
+        return false;
+    }
+}
+
+void createAlohaPacket(uint8_t *output, HeaderStruct *header, DataStruct *data)
+{
+    // assume user has already allocated memory for output
+    
+    // create header
+    if (header)
+    {
+        output[0] = header->no;
+        output[0] |= header->fid << 4;
+    }
+    
+    // fit data
+    if (data)
+    {
+        output[1] = data->pd0;
+        output[2] = data->pd1;
+    }
+    
+    // calculate CRC
+    if (header && data)
+    {
+        output[3] = crc8(output, 3);
+    }
+}
\ No newline at end of file