HAPSRG / UsaPack

usapack public

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

UsaPack.cpp@3:526f824b50ef, 2021-04-26 (annotated)

Committer:
cocorlow
Date:
Mon Apr 26 11:33:25 2021 +0000
Revision:
3:526f824b50ef
Parent:
2:1096fc8201a3
Child:
4:844eff5a2fa8
usausagi

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 3:526f824b50ef 10 receive_buffer[receive_index] = c;
cocorlow 3:526f824b50ef 11 receive_index = (receive_index + 1) % receive_size;
cocorlow 1:7d6e751f5986 12 if (c == '\0')
cocorlow 1:7d6e751f5986 13 {
cocorlow 1:7d6e751f5986 14 Decode();
cocorlow 3:526f824b50ef 15 receive_index = 0;
cocorlow 3:526f824b50ef 16 }
cocorlow 3:526f824b50ef 17 }
cocorlow 3:526f824b50ef 18 }
cocorlow 3:526f824b50ef 19
cocorlow 3:526f824b50ef 20 void UsaPack::SendByte()
cocorlow 3:526f824b50ef 21 {
cocorlow 3:526f824b50ef 22 while (serial.writable())
cocorlow 3:526f824b50ef 23 {
cocorlow 3:526f824b50ef 24 if (send_index != send_end_index)
cocorlow 3:526f824b50ef 25 {
cocorlow 3:526f824b50ef 26 serial.putc(send_buffer[send_index]);
cocorlow 3:526f824b50ef 27 send_index = (send_index + 1) % send_size;
cocorlow 3:526f824b50ef 28 }
cocorlow 3:526f824b50ef 29 else
cocorlow 3:526f824b50ef 30 {
cocorlow 3:526f824b50ef 31 serial.attach(NULL, Serial::TxIrq);
cocorlow 3:526f824b50ef 32 break;
cocorlow 1:7d6e751f5986 33 }
cocorlow 1:7d6e751f5986 34 }
cocorlow 1:7d6e751f5986 35 }
cocorlow 1:7d6e751f5986 36
cocorlow 1:7d6e751f5986 37 void UsaPack::Decode()
cocorlow 1:7d6e751f5986 38 {
cocorlow 3:526f824b50ef 39 char data[receive_size];
cocorlow 3:526f824b50ef 40 CobsDecode(receive_buffer, receive_index-2, data);
cocorlow 1:7d6e751f5986 41 union
cocorlow 1:7d6e751f5986 42 {
cocorlow 1:7d6e751f5986 43 char c[4];
cocorlow 1:7d6e751f5986 44 int a;
cocorlow 1:7d6e751f5986 45 } _address;
cocorlow 1:7d6e751f5986 46 for (int i = 0; i < 4; i++)
cocorlow 1:7d6e751f5986 47 {
cocorlow 1:7d6e751f5986 48 _address.c[i] = data[i];
cocorlow 1:7d6e751f5986 49 }
cocorlow 1:7d6e751f5986 50 for (int i = 0; i < package_index; i++)
cocorlow 1:7d6e751f5986 51 {
cocorlow 1:7d6e751f5986 52 if (package_address[i] == _address.a)
cocorlow 1:7d6e751f5986 53 {
cocorlow 1:7d6e751f5986 54 for (int j = 0; j < package_size[i]; j++)
cocorlow 1:7d6e751f5986 55 {
cocorlow 1:7d6e751f5986 56 *((char*)(package_object[i])+j) = data[j+4];
cocorlow 1:7d6e751f5986 57 }
cocorlow 1:7d6e751f5986 58 break;
cocorlow 1:7d6e751f5986 59 }
cocorlow 1:7d6e751f5986 60 }
cocorlow 0:8adbff0ff843 61 }
cocorlow 0:8adbff0ff843 62
cocorlow 0:8adbff0ff843 63 UsaPack::UsaPack(PinName tx, PinName rx, int baud)
cocorlow 3:526f824b50ef 64 :serial(tx, rx, baud), receive_index(0), package_index(0), send_index(0), send_end_index(0)
cocorlow 0:8adbff0ff843 65 {
cocorlow 0:8adbff0ff843 66 serial.attach(this, &UsaPack::Receive, Serial::RxIrq);
cocorlow 3:526f824b50ef 67 for (int i = 0; i < receive_size; i++)
cocorlow 0:8adbff0ff843 68 {
cocorlow 3:526f824b50ef 69 receive_buffer[i] = '\0';
cocorlow 0:8adbff0ff843 70 }
cocorlow 0:8adbff0ff843 71 }
cocorlow 0:8adbff0ff843 72
cocorlow 1:7d6e751f5986 73 void UsaPack::CobsEncode(char data[], int length, char send_data[])
cocorlow 0:8adbff0ff843 74 {
cocorlow 0:8adbff0ff843 75 int last_zero = 0;
cocorlow 0:8adbff0ff843 76 for (int i = 0; i < length; i++)
cocorlow 0:8adbff0ff843 77 {
cocorlow 0:8adbff0ff843 78 if (data[i] == 0)
cocorlow 0:8adbff0ff843 79 {
cocorlow 0:8adbff0ff843 80 send_data[last_zero] = i - last_zero + 1;
cocorlow 0:8adbff0ff843 81 last_zero = i + 1;
cocorlow 0:8adbff0ff843 82 }
cocorlow 0:8adbff0ff843 83 else
cocorlow 0:8adbff0ff843 84 {
cocorlow 0:8adbff0ff843 85 send_data[i + 1] = data[i];
cocorlow 0:8adbff0ff843 86 }
cocorlow 0:8adbff0ff843 87 }
cocorlow 0:8adbff0ff843 88 send_data[last_zero] = length - last_zero + 1;
cocorlow 0:8adbff0ff843 89 send_data[length + 1] = 0;
cocorlow 0:8adbff0ff843 90 }
cocorlow 0:8adbff0ff843 91
cocorlow 1:7d6e751f5986 92 void UsaPack::CobsDecode(char receive_data[], int length, char data[])
cocorlow 0:8adbff0ff843 93 {
cocorlow 0:8adbff0ff843 94 int next_zero;
cocorlow 0:8adbff0ff843 95 next_zero = receive_data[0] - 1;
cocorlow 0:8adbff0ff843 96 for (int i = 0; i < length; i++)
cocorlow 0:8adbff0ff843 97 {
cocorlow 0:8adbff0ff843 98 data[i] = receive_data[i + 1];
cocorlow 0:8adbff0ff843 99 }
cocorlow 0:8adbff0ff843 100 while (next_zero < length)
cocorlow 0:8adbff0ff843 101 {
cocorlow 0:8adbff0ff843 102 data[next_zero] = 0;
cocorlow 0:8adbff0ff843 103 next_zero += receive_data[next_zero + 1];
cocorlow 0:8adbff0ff843 104 }
cocorlow 0:8adbff0ff843 105 }
cocorlow 0:8adbff0ff843 106