usapack public
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: UsaPack.cpp
- Revision:
- 0:8adbff0ff843
- Child:
- 1:7d6e751f5986
diff -r 000000000000 -r 8adbff0ff843 UsaPack.cpp --- /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]; + } +} +