added more packet stuff

compile_commands.json

@@ -50,11 +50,11 @@
             "-Icfg",
             "-Iinc",
             "-o",
-            "build/p_serial_packet.o",
-            "src/p_serial_packet.cpp"
+            "build/main.o",
+            "src/main.cpp"
         ],
         "directory": "/storage/Shared/Projects/Penguinator/pi_serial_bus_manager",
-        "file": "src/p_serial_packet.cpp"
+        "file": "src/main.cpp"
     },
     {
         "arguments": [
@@ -69,10 +69,10 @@
             "-Icfg",
             "-Iinc",
             "-o",
-            "build/main.o",
-            "src/main.cpp"
+            "build/p_serial_packet.o",
+            "src/p_serial_packet.cpp"
         ],
         "directory": "/storage/Shared/Projects/Penguinator/pi_serial_bus_manager",
-        "file": "src/main.cpp"
+        "file": "src/p_serial_packet.cpp"
     }
 ]

inc/p_serial_packet.h

@@ -1,29 +1,41 @@
 #ifndef __P_SERIAL_PACKET_H__
 #define __P_SERIAL_PACKET_H__
 
+// start byte
 // src_addr {1} +
 // dest_addr {1} +
 // frame_length {1} +
 // checksum {1} +
-// frame_data {512}
+// frame_data {510} -- making it 510 instead of 511 so it is easily divisible
 #define MAX_MESSAGE_LEN (516)
-#define MAX_FRAME_DATA_LEN (512)
+#define MAX_FRAME_DATA_LEN (510)
 #define MASTER_ADDR (0x01)
 
 #include <cinttypes>
+#include <iostream>
 class ps_packet
 {
 public:
+	// for encoding
+	ps_packet(uint8_t _src_addr, uint8_t _dest_addr, uint8_t* _frame_data,
+			  uint8_t _frame_data_len);
+	// for decoding
 	ps_packet(uint8_t* data);
 	~ps_packet();
-	uint8_t src_addr;
-	uint8_t dest_addr;
-	uint8_t checksum;
+	uint8_t* src_addr;
+	uint8_t* dest_addr;
+	uint8_t* frame_data_len;
+	uint8_t* checksum;
+	uint8_t* frame_data;
 	uint8_t data[MAX_MESSAGE_LEN];
+	std::size_t msg_len;
 	bool encode();
 	bool decode();
+	static ps_packet from_xy(int x, int y);
 
 private:
+	bool stuff_bytes();
+	bool destuff_bytes();
 	bool validate();
 	void calc_checksum();
 };

src/main.cpp

@@ -8,6 +8,9 @@
 #define SERIAL_DEV ("/dev/ttyAMA1")
 #define ZMQ_IPC_ADDR ("ipc:///penguinator/pubsub")
 
+int x = 0;
+int y = 0;
+
 void sigint_handler(int param)
 {
 	std::cout << "Exiting..." << std::endl;
@@ -26,8 +29,10 @@ int main()
 	for (;;)
 	{
 		zmq::message_t msg;
-		sub.recv(&msg, 0);
-		std::cout << msg << std::endl;
+		if (sub.recv(&msg, 0))
+		{
+			std::cout << msg << std::endl;
+		}
 	}
 
 	return 0;

src/p_serial_packet.cpp

@@ -1,8 +1,22 @@
 #include "p_serial_packet.h"
+#include <iostream>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
 
 ps_packet::ps_packet(uint8_t* data)
 {}
 
+ps_packet::ps_packet(uint8_t _src_addr, uint8_t _dest_addr,
+					 uint8_t* _frame_data, uint8_t _frame_data_len)
+{
+	data[0] = 0x7E;
+	src_addr = &data[1];
+	dest_addr = &data[2];
+	frame_data_len = &data[3];
+	checksum = nullptr;
+	memcpy(frame_data, _frame_data, MAX_FRAME_DATA_LEN);
+}
 ps_packet::~ps_packet()
 {}
 
@@ -38,3 +52,48 @@ 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;
+	int new_ind = 0;
+	uint8_t temp_buffer[MAX_FRAME_DATA_LEN];
+	memcpy(frame_data, temp_buffer, MAX_FRAME_DATA_LEN);
+	msg_len = *frame_data_len + 5;
+	for (; old_ind < *frame_data_len; old_ind++)
+	{
+		if (temp_buffer[old_ind] == 0x7E || temp_buffer[old_ind] == 0x7D)
+		{
+			msg_len++;
+			frame_data[new_ind++] = 0x7D;
+			frame_data[new_ind] = temp_buffer[old_ind];
+		}
+		else
+		{
+			frame_data[new_ind] = temp_buffer[old_ind];
+		}
+	}
+	if (msg_len > MAX_MESSAGE_LEN)
+	{
+		return false;
+	}
+	return true;
+}
+
+bool ps_packet::destuff_bytes()
+{
+	return false;
+}