HAPSRG / UsaPack

usapack public

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

UsaPack.cpp@4:844eff5a2fa8, 2021-05-23 (annotated)

Committer:
cocorlow
Date:
Sun May 23 23:12:43 2021 +0000
Revision:
4:844eff5a2fa8
Parent:
3:526f824b50ef 
checksum;

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 4:844eff5a2fa8 41 union c4
cocorlow 1:7d6e751f5986 42 {
cocorlow 1:7d6e751f5986 43 char c[4];
cocorlow 1:7d6e751f5986 44 int a;
cocorlow 4:844eff5a2fa8 45 };
cocorlow 4:844eff5a2fa8 46 c4 _address;
cocorlow 4:844eff5a2fa8 47 c4 _checksum;
cocorlow 1:7d6e751f5986 48 for (int i = 0; i < 4; i++)
cocorlow 1:7d6e751f5986 49 {
cocorlow 1:7d6e751f5986 50 _address.c[i] = data[i];
cocorlow 1:7d6e751f5986 51 }
cocorlow 4:844eff5a2fa8 52 for (int i = 0; i < 4; i++)
cocorlow 4:844eff5a2fa8 53 {
cocorlow 4:844eff5a2fa8 54 _checksum.c[i] = data[i+4];
cocorlow 4:844eff5a2fa8 55 }
cocorlow 1:7d6e751f5986 56 for (int i = 0; i < package_index; i++)
cocorlow 1:7d6e751f5986 57 {
cocorlow 1:7d6e751f5986 58 if (package_address[i] == _address.a)
cocorlow 1:7d6e751f5986 59 {
cocorlow 4:844eff5a2fa8 60 int checksum = 0;
cocorlow 1:7d6e751f5986 61 for (int j = 0; j < package_size[i]; j++)
cocorlow 1:7d6e751f5986 62 {
cocorlow 4:844eff5a2fa8 63 checksum += data[j+8];
cocorlow 4:844eff5a2fa8 64 }
cocorlow 4:844eff5a2fa8 65 if (_checksum.a != checksum)
cocorlow 4:844eff5a2fa8 66 {
cocorlow 4:844eff5a2fa8 67 return;
cocorlow 4:844eff5a2fa8 68 }
cocorlow 4:844eff5a2fa8 69 for (int j = 0; j < package_size[i]; j++)
cocorlow 4:844eff5a2fa8 70 {
cocorlow 4:844eff5a2fa8 71 *((char*)(package_object[i])+j) = data[j+8];
cocorlow 1:7d6e751f5986 72 }
cocorlow 1:7d6e751f5986 73 break;
cocorlow 1:7d6e751f5986 74 }
cocorlow 1:7d6e751f5986 75 }
cocorlow 0:8adbff0ff843 76 }
cocorlow 0:8adbff0ff843 77
cocorlow 0:8adbff0ff843 78 UsaPack::UsaPack(PinName tx, PinName rx, int baud)
cocorlow 3:526f824b50ef 79 :serial(tx, rx, baud), receive_index(0), package_index(0), send_index(0), send_end_index(0)
cocorlow 0:8adbff0ff843 80 {
cocorlow 0:8adbff0ff843 81 serial.attach(this, &UsaPack::Receive, Serial::RxIrq);
cocorlow 3:526f824b50ef 82 for (int i = 0; i < receive_size; i++)
cocorlow 0:8adbff0ff843 83 {
cocorlow 3:526f824b50ef 84 receive_buffer[i] = '\0';
cocorlow 0:8adbff0ff843 85 }
cocorlow 0:8adbff0ff843 86 }
cocorlow 0:8adbff0ff843 87
cocorlow 1:7d6e751f5986 88 void UsaPack::CobsEncode(char data[], int length, char send_data[])
cocorlow 0:8adbff0ff843 89 {
cocorlow 0:8adbff0ff843 90 int last_zero = 0;
cocorlow 0:8adbff0ff843 91 for (int i = 0; i < length; i++)
cocorlow 0:8adbff0ff843 92 {
cocorlow 0:8adbff0ff843 93 if (data[i] == 0)
cocorlow 0:8adbff0ff843 94 {
cocorlow 0:8adbff0ff843 95 send_data[last_zero] = i - last_zero + 1;
cocorlow 0:8adbff0ff843 96 last_zero = i + 1;
cocorlow 0:8adbff0ff843 97 }
cocorlow 0:8adbff0ff843 98 else
cocorlow 0:8adbff0ff843 99 {
cocorlow 0:8adbff0ff843 100 send_data[i + 1] = data[i];
cocorlow 0:8adbff0ff843 101 }
cocorlow 0:8adbff0ff843 102 }
cocorlow 0:8adbff0ff843 103 send_data[last_zero] = length - last_zero + 1;
cocorlow 0:8adbff0ff843 104 send_data[length + 1] = 0;
cocorlow 0:8adbff0ff843 105 }
cocorlow 0:8adbff0ff843 106
cocorlow 1:7d6e751f5986 107 void UsaPack::CobsDecode(char receive_data[], int length, char data[])
cocorlow 0:8adbff0ff843 108 {
cocorlow 0:8adbff0ff843 109 int next_zero;
cocorlow 0:8adbff0ff843 110 next_zero = receive_data[0] - 1;
cocorlow 0:8adbff0ff843 111 for (int i = 0; i < length; i++)
cocorlow 0:8adbff0ff843 112 {
cocorlow 0:8adbff0ff843 113 data[i] = receive_data[i + 1];
cocorlow 0:8adbff0ff843 114 }
cocorlow 0:8adbff0ff843 115 while (next_zero < length)
cocorlow 0:8adbff0ff843 116 {
cocorlow 0:8adbff0ff843 117 data[next_zero] = 0;
cocorlow 0:8adbff0ff843 118 next_zero += receive_data[next_zero + 1];
cocorlow 0:8adbff0ff843 119 }
cocorlow 0:8adbff0ff843 120 }
cocorlow 0:8adbff0ff843 121