byte stuffing works, checksum works afaik, tested encoded packets

stable
Penguin 3 years ago
parent 13d20ad088
commit b7b5ccd428

Binary file not shown.

Binary file not shown.

Binary file not shown.

@ -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": [

@ -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

@ -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");
}

@ -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

Loading…
Cancel
Save