HAPSRG / UsaPack

usapack public

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

UsaPack.cpp@2:1096fc8201a3, 2021-04-24 (annotated)

Committer:
cocorlow
Date:
Sat Apr 24 04:38:39 2021 +0000
Revision:
2:1096fc8201a3
Parent:
1:7d6e751f5986
Child:
3:526f824b50ef
index

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