usapack public
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: UsaPack.cpp
- Revision:
- 1:7d6e751f5986
- Parent:
- 0:8adbff0ff843
- Child:
- 2:1096fc8201a3
diff -r 8adbff0ff843 -r 7d6e751f5986 UsaPack.cpp --- a/UsaPack.cpp Fri Apr 23 18:53:23 2021 +0000 +++ b/UsaPack.cpp Sat Apr 24 04:18:05 2021 +0000 @@ -3,7 +3,43 @@ void UsaPack::Receive() { - + while (serial.readable()) + { + char c; + c = serial.getc(); + uart_buffer[uart_index] = c; + uart_index = (uart_index + 1) % uart_size; + if (c == '\0') + { + Decode(); + } + } +} + +void UsaPack::Decode() +{ + char data[uart_size]; + CobsDecode(uart_buffer, uart_index-2, data); + union + { + char c[4]; + int a; + } _address; + for (int i = 0; i < 4; i++) + { + _address.c[i] = data[i]; + } + for (int i = 0; i < package_index; i++) + { + if (package_address[i] == _address.a) + { + for (int j = 0; j < package_size[i]; j++) + { + *((char*)(package_object[i])+j) = data[j+4]; + } + break; + } + } } UsaPack::UsaPack(PinName tx, PinName rx, int baud) @@ -16,7 +52,7 @@ } } -void UsaPack::CobsEncode(uint8_t data[], int length, uint8_t send_data[]) +void UsaPack::CobsEncode(char data[], int length, char send_data[]) { int last_zero = 0; for (int i = 0; i < length; i++) @@ -35,7 +71,7 @@ send_data[length + 1] = 0; } -void UsaPack::CobsDecode(uint8_t receive_data[], int length, uint8_t data[]) +void UsaPack::CobsDecode(char receive_data[], int length, char data[]) { int next_zero; next_zero = receive_data[0] - 1;