HAPSRG / UsaPack

usapack public

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

UsaPack.cpp@0:8adbff0ff843, 2021-04-23 (annotated)

Committer:
cocorlow
Date:
Fri Apr 23 18:53:23 2021 +0000
Revision:
0:8adbff0ff843
Child:
1:7d6e751f5986
UART COBS

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cocorlow 0:8adbff0ff843 1 #include "UsaPack.hpp"
cocorlow 0:8adbff0ff843 2 #include "mbed.h"
cocorlow 0:8adbff0ff843 3
cocorlow 0:8adbff0ff843 4 void UsaPack::Receive()
cocorlow 0:8adbff0ff843 5 {
cocorlow 0:8adbff0ff843 6
cocorlow 0:8adbff0ff843 7 }
cocorlow 0:8adbff0ff843 8
cocorlow 0:8adbff0ff843 9 UsaPack::UsaPack(PinName tx, PinName rx, int baud)
cocorlow 0:8adbff0ff843 10 :serial(tx, rx, baud), uart_index(0), package_index(0)
cocorlow 0:8adbff0ff843 11 {
cocorlow 0:8adbff0ff843 12 serial.attach(this, &UsaPack::Receive, Serial::RxIrq);
cocorlow 0:8adbff0ff843 13 for (int i = 0; i < uart_size; i++)
cocorlow 0:8adbff0ff843 14 {
cocorlow 0:8adbff0ff843 15 uart_buffer[i] = '\0';
cocorlow 0:8adbff0ff843 16 }
cocorlow 0:8adbff0ff843 17 }
cocorlow 0:8adbff0ff843 18
cocorlow 0:8adbff0ff843 19 void UsaPack::CobsEncode(uint8_t data[], int length, uint8_t send_data[])
cocorlow 0:8adbff0ff843 20 {
cocorlow 0:8adbff0ff843 21 int last_zero = 0;
cocorlow 0:8adbff0ff843 22 for (int i = 0; i < length; i++)
cocorlow 0:8adbff0ff843 23 {
cocorlow 0:8adbff0ff843 24 if (data[i] == 0)
cocorlow 0:8adbff0ff843 25 {
cocorlow 0:8adbff0ff843 26 send_data[last_zero] = i - last_zero + 1;
cocorlow 0:8adbff0ff843 27 last_zero = i + 1;
cocorlow 0:8adbff0ff843 28 }
cocorlow 0:8adbff0ff843 29 else
cocorlow 0:8adbff0ff843 30 {
cocorlow 0:8adbff0ff843 31 send_data[i + 1] = data[i];
cocorlow 0:8adbff0ff843 32 }
cocorlow 0:8adbff0ff843 33 }
cocorlow 0:8adbff0ff843 34 send_data[last_zero] = length - last_zero + 1;
cocorlow 0:8adbff0ff843 35 send_data[length + 1] = 0;
cocorlow 0:8adbff0ff843 36 }
cocorlow 0:8adbff0ff843 37
cocorlow 0:8adbff0ff843 38 void UsaPack::CobsDecode(uint8_t receive_data[], int length, uint8_t data[])
cocorlow 0:8adbff0ff843 39 {
cocorlow 0:8adbff0ff843 40 int next_zero;
cocorlow 0:8adbff0ff843 41 next_zero = receive_data[0] - 1;
cocorlow 0:8adbff0ff843 42 for (int i = 0; i < length; i++)
cocorlow 0:8adbff0ff843 43 {
cocorlow 0:8adbff0ff843 44 data[i] = receive_data[i + 1];
cocorlow 0:8adbff0ff843 45 }
cocorlow 0:8adbff0ff843 46 while (next_zero < length)
cocorlow 0:8adbff0ff843 47 {
cocorlow 0:8adbff0ff843 48 data[next_zero] = 0;
cocorlow 0:8adbff0ff843 49 next_zero += receive_data[next_zero + 1];
cocorlow 0:8adbff0ff843 50 }
cocorlow 0:8adbff0ff843 51 }
cocorlow 0:8adbff0ff843 52