encoding works, need to do byte stuffing and checksum

src/main.cpp

@@ -1,8 +1,10 @@
 #include "p_serial_bus.h"
+#include "p_serial_packet.h"
 #include <iostream>
 #include <signal.h>
 #include <stdio.h>
 #include <stdlib.h>
+#include <string>
 #include <zmq.hpp>
 
 #define SERIAL_DEV ("/dev/ttyAMA1")
@@ -29,9 +31,20 @@ int main()
 	for (;;)
 	{
 		zmq::message_t msg;
+		int32_t x = 0;
+		int32_t y = 0;
 		if (sub.recv(&msg, 0))
 		{
-			std::cout << msg << std::endl;
+			uint8_t xy[64];
+			memset(xy, 0, 64);
+			memcpy(xy, msg.data(), msg.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++)
+			{
+				printf("[%d]: %02x ", ind, xy_packet.data[ind]);
+			}
+			printf("\n");
 		}
 	}
 

src/p_serial_packet.cpp

@@ -14,8 +14,16 @@ ps_packet::ps_packet(uint8_t _src_addr, uint8_t _dest_addr,
 	src_addr = &data[1];
 	dest_addr = &data[2];
 	frame_data_len = &data[3];
+	frame_data = &data[4];
+	msg_len = 5 + _frame_data_len;
 	checksum = nullptr;
-	memcpy(frame_data, _frame_data, MAX_FRAME_DATA_LEN);
+
+	*src_addr = _src_addr;
+	*dest_addr = _dest_addr;
+	*frame_data_len = _frame_data_len;
+
+	memset(frame_data, 0, MAX_FRAME_DATA_LEN);
+	memcpy(frame_data, _frame_data, _frame_data_len);
 }
 ps_packet::~ps_packet()
 {}
@@ -53,19 +61,6 @@ bool ps_packet::validate()
 void ps_packet::calc_checksum()
 {}
 
-ps_packet ps_packet::from_xy(int32_t x, int32_t y)
-{
-	uint8_t frame_data[MAX_FRAME_DATA_LEN];
-	memset(frame_data, 0, MAX_FRAME_DATA_LEN);
-	// op command
-	// 0x01 = set xy
-	frame_data[0] = 0x01;					   // + 1
-	memcpy(&frame_data[1], &x, 4;			   // + sizeof(x)
-	memcpy(&frame_data[5], &y, 4); // + sizeof(y)
-
-	return ps_packet(MASTER_ADDR, 0x02, frame_data, 1 + sizeof(x) + sizeof(y));
-}
-
 bool ps_packet::stuff_bytes()
 {
 	int old_ind = 0;
@@ -97,3 +92,16 @@ bool ps_packet::destuff_bytes()
 {
 	return false;
 }
+
+ps_packet ps_packet::from_xy(int32_t x, int32_t y)
+{
+	uint8_t frame_data[MAX_FRAME_DATA_LEN];
+	memset(frame_data, 0, MAX_FRAME_DATA_LEN);
+	// op command
+	// 0x01 = set xy
+	frame_data[0] = 0x01;		   // + 1
+	memcpy(&frame_data[1], &x, 4); // + sizeof(x)
+	memcpy(&frame_data[5], &y, 4); // + sizeof(y)
+
+	return ps_packet(MASTER_ADDR, 0x02, frame_data, 1 + sizeof(x) + sizeof(y));
+}