diff --git a/.cache/clangd/index/main.cpp.6C7D4E708A9CF215.idx b/.cache/clangd/index/main.cpp.6C7D4E708A9CF215.idx index b2a2d0b..97bbccf 100644 Binary files a/.cache/clangd/index/main.cpp.6C7D4E708A9CF215.idx and b/.cache/clangd/index/main.cpp.6C7D4E708A9CF215.idx differ diff --git a/.cache/clangd/index/p_serial_packet.cpp.45B1DE7F1E66FFCC.idx b/.cache/clangd/index/p_serial_packet.cpp.45B1DE7F1E66FFCC.idx index 97f1cdf..b74c9a3 100644 Binary files a/.cache/clangd/index/p_serial_packet.cpp.45B1DE7F1E66FFCC.idx and b/.cache/clangd/index/p_serial_packet.cpp.45B1DE7F1E66FFCC.idx differ diff --git a/.cache/clangd/index/p_serial_packet.h.38E83F6A61EFB6BA.idx b/.cache/clangd/index/p_serial_packet.h.38E83F6A61EFB6BA.idx index 56e0319..2f80e9b 100644 Binary files a/.cache/clangd/index/p_serial_packet.h.38E83F6A61EFB6BA.idx and b/.cache/clangd/index/p_serial_packet.h.38E83F6A61EFB6BA.idx differ diff --git a/bin/sbm b/bin/sbm index dd731f6..0d4b01c 100644 Binary files a/bin/sbm and b/bin/sbm differ diff --git a/build/main.o b/build/main.o index 856576e..8b5bfed 100644 Binary files a/build/main.o and b/build/main.o differ diff --git a/build/p_serial_packet.o b/build/p_serial_packet.o index 006df43..daec700 100644 Binary files a/build/p_serial_packet.o and b/build/p_serial_packet.o differ diff --git a/compile_commands.json b/compile_commands.json index 36301eb..44ba02e 100644 --- a/compile_commands.json +++ b/compile_commands.json @@ -12,11 +12,11 @@ "-Icfg", "-Iinc", "-o", - "build/p_idp.o", - "src/p_idp.cpp" + "build/p_serial_bus.o", + "src/p_serial_bus.cpp" ], "directory": "/storage/Shared/Projects/Penguinator/pi_serial_bus_manager", - "file": "src/p_idp.cpp" + "file": "src/p_serial_bus.cpp" }, { "arguments": [ @@ -31,11 +31,11 @@ "-Icfg", "-Iinc", "-o", - "build/p_serial_bus.o", - "src/p_serial_bus.cpp" + "build/p_idp.o", + "src/p_idp.cpp" ], "directory": "/storage/Shared/Projects/Penguinator/pi_serial_bus_manager", - "file": "src/p_serial_bus.cpp" + "file": "src/p_idp.cpp" }, { "arguments": [ diff --git a/inc/p_serial_packet.h b/inc/p_serial_packet.h index ef25956..ca0d856 100644 --- a/inc/p_serial_packet.h +++ b/inc/p_serial_packet.h @@ -32,11 +32,15 @@ public: bool encode(); bool decode(); static ps_packet from_xy(int x, int y); + static ps_packet test_packet(); +#ifdef DEBUG + void show_packet(); +#endif private: bool stuff_bytes(); bool destuff_bytes(); bool validate(); - void calc_checksum(); + uint8_t calc_checksum(); }; #endif diff --git a/src/main.cpp b/src/main.cpp index eb61b24..178fc23 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -28,21 +28,25 @@ int main() sub.connect(ZMQ_IPC_ADDR); sub.setsockopt(ZMQ_SUBSCRIBE, "", 0); + zmq::message_t msg_recv; + zmq::message_t msg_send; for (;;) { - zmq::message_t msg; int32_t x = 0; int32_t y = 0; - if (sub.recv(&msg, 0)) + if (sub.recv(&msg_recv, 0)) { uint8_t xy[64]; memset(xy, 0, 64); - memcpy(xy, msg.data(), msg.size()); + memcpy(xy, msg_recv.data(), msg_recv.size()); sscanf((const char*)xy, "X:%" PRId32 " Y:%" PRId32 "\n", &x, &y); ps_packet xy_packet = ps_packet::from_xy(x, y); - for (int ind = 0; ind < xy_packet.msg_len; ind++) + xy_packet.encode(); + xy_packet.show_packet(); + + for (size_t ind = 0; ind < xy_packet.msg_len; ind++) { - printf("[%d]: %02x ", ind, xy_packet.data[ind]); + printf("[%lu]: %02x ", ind, xy_packet.data[ind]); } printf("\n"); } diff --git a/src/p_serial_packet.cpp b/src/p_serial_packet.cpp index 754871a..4a9460b 100644 --- a/src/p_serial_packet.cpp +++ b/src/p_serial_packet.cpp @@ -34,7 +34,14 @@ bool ps_packet::encode() bool ret = true; do { - + uint8_t csum = calc_checksum(); + if (!stuff_bytes()) + { + ret = false; + break; + } + checksum = &data[msg_len - 1]; + *checksum = csum; } while (0); return ret; } @@ -44,11 +51,23 @@ bool ps_packet::decode() bool ret = true; do { + if (!destuff_bytes()) + { + ret = false; + break; + } + + if (!validate()) + { + ret = false; + break; + } } while (0); return ret; } +// done after receiving a packet and destuffing it bool ps_packet::validate() { bool ret = true; @@ -58,15 +77,25 @@ bool ps_packet::validate() } while (0); return ret; } -void ps_packet::calc_checksum() -{} + +uint8_t ps_packet::calc_checksum() +{ + uint32_t ret = 0; + for (size_t ind = 0; ind < *frame_data_len; ind++) + { + ret += frame_data[ind]; + } + ret &= 0xFF; + ret = 0xFF - ret; + return (uint8_t)ret; +} bool ps_packet::stuff_bytes() { int old_ind = 0; int new_ind = 0; - uint8_t temp_buffer[MAX_FRAME_DATA_LEN]; - memcpy(frame_data, temp_buffer, MAX_FRAME_DATA_LEN); + uint8_t temp_buffer[MAX_FRAME_DATA_LEN] = {0}; + memcpy(temp_buffer, frame_data, MAX_FRAME_DATA_LEN); msg_len = *frame_data_len + 5; for (; old_ind < *frame_data_len; old_ind++) { @@ -74,11 +103,11 @@ bool ps_packet::stuff_bytes() { msg_len++; frame_data[new_ind++] = 0x7D; - frame_data[new_ind] = temp_buffer[old_ind]; + frame_data[new_ind++] = temp_buffer[old_ind] ^ 0x20; } else { - frame_data[new_ind] = temp_buffer[old_ind]; + frame_data[new_ind++] = temp_buffer[old_ind]; } } if (msg_len > MAX_MESSAGE_LEN) @@ -105,3 +134,29 @@ ps_packet ps_packet::from_xy(int32_t x, int32_t y) return ps_packet(MASTER_ADDR, 0x02, frame_data, 1 + sizeof(x) + sizeof(y)); } + +ps_packet ps_packet::test_packet() +{ + uint8_t frame_data[16] = {0xFF, 0x01, 0x02, 0x03, 0x04, 0x7D, 0x7D, 0x07, + 0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0F, 0x7E}; + + return ps_packet(MASTER_ADDR, 0xFF, frame_data, 16); +} + +#ifdef DEBUG +void ps_packet::show_packet() +{ + printf("Start Byte:\t\t0x%02x\n", data[0]); + printf("Source Address:\t\t0x%02x\n", *src_addr); + printf("Dest Address:\t\t0x%02x\n", *dest_addr); + printf("Frame Data Len:\t\t0x%02x\n", *frame_data_len); + printf("Frame Data:\n"); + for (size_t ind = 0; ind < msg_len - 4; ind++) + { + printf("\t\t[%2lu]: 0x%02x\n", ind, frame_data[(int)ind]); + } + printf("\n"); + printf("Message Length: %lu\n", msg_len); + printf("Checksum: %0x02x\n", *checksum); +} +#endif