HAPSRG / UsaPack

usapack public

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

UsaPack.cpp@1:7d6e751f5986, 2021-04-24 (annotated)

Committer:
cocorlow
Date:
Sat Apr 24 04:18:05 2021 +0000
Revision:
1:7d6e751f5986
Parent:
0:8adbff0ff843
Child:
2:1096fc8201a3
subscribe;

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 1:7d6e751f5986 6 while (serial.readable())
cocorlow 1:7d6e751f5986 7 {
cocorlow 1:7d6e751f5986 8 char c;
cocorlow 1:7d6e751f5986 9 c = serial.getc();
cocorlow 1:7d6e751f5986 10 uart_buffer[uart_index] = c;
cocorlow 1:7d6e751f5986 11 uart_index = (uart_index + 1) % uart_size;
cocorlow 1:7d6e751f5986 12 if (c == '\0')
cocorlow 1:7d6e751f5986 13 {
cocorlow 1:7d6e751f5986 14 Decode();
cocorlow 1:7d6e751f5986 15 }
cocorlow 1:7d6e751f5986 16 }
cocorlow 1:7d6e751f5986 17 }
cocorlow 1:7d6e751f5986 18
cocorlow 1:7d6e751f5986 19 void UsaPack::Decode()
cocorlow 1:7d6e751f5986 20 {
cocorlow 1:7d6e751f5986 21 char data[uart_size];
cocorlow 1:7d6e751f5986 22 CobsDecode(uart_buffer, uart_index-2, data);
cocorlow 1:7d6e751f5986 23 union
cocorlow 1:7d6e751f5986 24 {
cocorlow 1:7d6e751f5986 25 char c[4];
cocorlow 1:7d6e751f5986 26 int a;
cocorlow 1:7d6e751f5986 27 } _address;
cocorlow 1:7d6e751f5986 28 for (int i = 0; i < 4; i++)
cocorlow 1:7d6e751f5986 29 {
cocorlow 1:7d6e751f5986 30 _address.c[i] = data[i];
cocorlow 1:7d6e751f5986 31 }
cocorlow 1:7d6e751f5986 32 for (int i = 0; i < package_index; i++)
cocorlow 1:7d6e751f5986 33 {
cocorlow 1:7d6e751f5986 34 if (package_address[i] == _address.a)
cocorlow 1:7d6e751f5986 35 {
cocorlow 1:7d6e751f5986 36 for (int j = 0; j < package_size[i]; j++)
cocorlow 1:7d6e751f5986 37 {
cocorlow 1:7d6e751f5986 38 *((char*)(package_object[i])+j) = data[j+4];
cocorlow 1:7d6e751f5986 39 }
cocorlow 1:7d6e751f5986 40 break;
cocorlow 1:7d6e751f5986 41 }
cocorlow 1:7d6e751f5986 42 }
cocorlow 0:8adbff0ff843 43 }
cocorlow 0:8adbff0ff843 44
cocorlow 0:8adbff0ff843 45 UsaPack::UsaPack(PinName tx, PinName rx, int baud)
cocorlow 0:8adbff0ff843 46 :serial(tx, rx, baud), uart_index(0), package_index(0)
cocorlow 0:8adbff0ff843 47 {
cocorlow 0:8adbff0ff843 48 serial.attach(this, &UsaPack::Receive, Serial::RxIrq);
cocorlow 0:8adbff0ff843 49 for (int i = 0; i < uart_size; i++)
cocorlow 0:8adbff0ff843 50 {
cocorlow 0:8adbff0ff843 51 uart_buffer[i] = '\0';
cocorlow 0:8adbff0ff843 52 }
cocorlow 0:8adbff0ff843 53 }
cocorlow 0:8adbff0ff843 54
cocorlow 1:7d6e751f5986 55 void UsaPack::CobsEncode(char data[], int length, char send_data[])
cocorlow 0:8adbff0ff843 56 {
cocorlow 0:8adbff0ff843 57 int last_zero = 0;
cocorlow 0:8adbff0ff843 58 for (int i = 0; i < length; i++)
cocorlow 0:8adbff0ff843 59 {
cocorlow 0:8adbff0ff843 60 if (data[i] == 0)
cocorlow 0:8adbff0ff843 61 {
cocorlow 0:8adbff0ff843 62 send_data[last_zero] = i - last_zero + 1;
cocorlow 0:8adbff0ff843 63 last_zero = i + 1;
cocorlow 0:8adbff0ff843 64 }
cocorlow 0:8adbff0ff843 65 else
cocorlow 0:8adbff0ff843 66 {
cocorlow 0:8adbff0ff843 67 send_data[i + 1] = data[i];
cocorlow 0:8adbff0ff843 68 }
cocorlow 0:8adbff0ff843 69 }
cocorlow 0:8adbff0ff843 70 send_data[last_zero] = length - last_zero + 1;
cocorlow 0:8adbff0ff843 71 send_data[length + 1] = 0;
cocorlow 0:8adbff0ff843 72 }
cocorlow 0:8adbff0ff843 73
cocorlow 1:7d6e751f5986 74 void UsaPack::CobsDecode(char receive_data[], int length, char data[])
cocorlow 0:8adbff0ff843 75 {
cocorlow 0:8adbff0ff843 76 int next_zero;
cocorlow 0:8adbff0ff843 77 next_zero = receive_data[0] - 1;
cocorlow 0:8adbff0ff843 78 for (int i = 0; i < length; i++)
cocorlow 0:8adbff0ff843 79 {
cocorlow 0:8adbff0ff843 80 data[i] = receive_data[i + 1];
cocorlow 0:8adbff0ff843 81 }
cocorlow 0:8adbff0ff843 82 while (next_zero < length)
cocorlow 0:8adbff0ff843 83 {
cocorlow 0:8adbff0ff843 84 data[next_zero] = 0;
cocorlow 0:8adbff0ff843 85 next_zero += receive_data[next_zero + 1];
cocorlow 0:8adbff0ff843 86 }
cocorlow 0:8adbff0ff843 87 }
cocorlow 0:8adbff0ff843 88