Diff: UsaPack.cpp

Revision:

0:8adbff0ff843

Child:

1:7d6e751f5986

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/UsaPack.cpp	Fri Apr 23 18:53:23 2021 +0000
@@ -0,0 +1,52 @@
+#include "UsaPack.hpp"
+#include "mbed.h"
+
+void UsaPack::Receive()
+{
+    
+}
+
+UsaPack::UsaPack(PinName tx, PinName rx, int baud)
+    :serial(tx, rx, baud), uart_index(0), package_index(0)
+{
+    serial.attach(this, &UsaPack::Receive, Serial::RxIrq);
+    for (int i = 0; i < uart_size; i++)
+    {
+        uart_buffer[i] = '\0';
+    }
+}
+
+void UsaPack::CobsEncode(uint8_t data[], int length, uint8_t send_data[])
+{
+    int last_zero = 0;
+    for (int i = 0; i < length; i++)
+    {
+        if (data[i] == 0)
+        {
+            send_data[last_zero] = i - last_zero + 1;
+            last_zero = i + 1;
+        }
+        else
+        {
+            send_data[i + 1] = data[i];
+        }
+    }
+    send_data[last_zero] = length - last_zero + 1;
+    send_data[length + 1] = 0;
+}
+
+void UsaPack::CobsDecode(uint8_t receive_data[], int length, uint8_t data[])
+{
+    int next_zero;
+    next_zero = receive_data[0] - 1;
+    for (int i = 0; i < length; i++)
+    {
+        data[i] = receive_data[i + 1];
+    }
+    while (next_zero < length)
+    {
+        data[next_zero] = 0;
+        next_zero += receive_data[next_zero + 1];
+    }
+}
+