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?
User | Revision | Line number | New 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 |