added some tests to verify packet encoding and decoding was working properly

.gdb_history

@@ -0,0 +1,5 @@
+info source
+b main
+r
+info source
+q

Makefile

@@ -2,6 +2,7 @@ TARGET=sbm
 BUILD_DIR=build
 MK_DIR=mkdir -p
 
+
 CPP_SRCS= \
 	src/main.cpp \
 	src/p_serial_bus.cpp \
@@ -15,7 +16,24 @@ SIZE=$(TOOLCHAIN)-size
 INCLUDES=\
 	 -Icfg\
 	 -Iinc
-CFLAGS=-Og -Wall -fdata-sections -ffunction-sections -g3 -DDEBUG $(INCLUDES) -lzmq
+CFLAGS=-Og \
+	   -Wall \
+	   -fdata-sections \
+	   -ffunction-sections \
+	   -std=gnu++11 \
+	   -g3 \
+	   -DDEBUG \
+	   $(INCLUDES) \
+	   -Wextra \
+	   -Werror \
+	   -Wconversion \
+	   -lzmq
+
+ifdef RUN_TESTS
+	CPP_SRCS+=tests/tests.cpp
+	INCLUDES+=-Itests
+	CFLAGS+=-DRUN_TESTS
+endif
 
 all: bin/$(TARGET)
 

compile_commands.json

@@ -7,15 +7,88 @@
             "-Wall",
             "-fdata-sections",
             "-ffunction-sections",
+            "-std=gnu++11",
             "-g3",
             "-DDEBUG",
             "-Icfg",
             "-Iinc",
+            "-Wextra",
+            "-Werror",
+            "-Wconversion",
             "-o",
             "build/p_serial_bus.o",
             "src/p_serial_bus.cpp"
         ],
         "directory": "/storage/Shared/Projects/Penguinator/pi_serial_bus_manager",
         "file": "src/p_serial_bus.cpp"
+    },
+    {
+        "arguments": [
+            "g++",
+            "-c",
+            "-Og",
+            "-Wall",
+            "-fdata-sections",
+            "-ffunction-sections",
+            "-std=gnu++11",
+            "-g3",
+            "-DDEBUG",
+            "-Icfg",
+            "-Iinc",
+            "-Wextra",
+            "-Werror",
+            "-Wconversion",
+            "-o",
+            "build/p_idp.o",
+            "src/p_idp.cpp"
+        ],
+        "directory": "/storage/Shared/Projects/Penguinator/pi_serial_bus_manager",
+        "file": "src/p_idp.cpp"
+    },
+    {
+        "arguments": [
+            "g++",
+            "-c",
+            "-Og",
+            "-Wall",
+            "-fdata-sections",
+            "-ffunction-sections",
+            "-std=gnu++11",
+            "-g3",
+            "-DDEBUG",
+            "-Icfg",
+            "-Iinc",
+            "-Wextra",
+            "-Werror",
+            "-Wconversion",
+            "-o",
+            "build/main.o",
+            "src/main.cpp"
+        ],
+        "directory": "/storage/Shared/Projects/Penguinator/pi_serial_bus_manager",
+        "file": "src/main.cpp"
+    },
+    {
+        "arguments": [
+            "g++",
+            "-c",
+            "-Og",
+            "-Wall",
+            "-fdata-sections",
+            "-ffunction-sections",
+            "-std=gnu++11",
+            "-g3",
+            "-DDEBUG",
+            "-Icfg",
+            "-Iinc",
+            "-Wextra",
+            "-Werror",
+            "-Wconversion",
+            "-o",
+            "build/p_serial_packet.o",
+            "src/p_serial_packet.cpp"
+        ],
+        "directory": "/storage/Shared/Projects/Penguinator/pi_serial_bus_manager",
+        "file": "src/p_serial_packet.cpp"
     }
 ]

env.sh

@@ -1,6 +1,7 @@
 alias pipush="rsync -ar \
 	--include='src**' \
 	--include='inc**' \
+	--include='Makefile' \
 	--exclude='*' \
 	~/Projects/Penguinator/pi_serial_bus_manager/ penguin@192.168.1.121:~/pi_serial_bus_manager"
 alias pipull="rsync -ar penguin@192.168.1.121:~/pi_serial_bus_manager/* ~/Projects/Penguinator/pi_serial_bus_manager"

inc/p_serial_packet.h

@@ -20,7 +20,7 @@ public:
 	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* _msg, size_t _msg_len);
 	~ps_packet();
 	uint8_t* src_addr;
 	uint8_t* dest_addr;
@@ -32,8 +32,9 @@ public:
 	bool encode();
 	bool decode();
 	static ps_packet from_xy(int x, int y);
-	static ps_packet test_packet();
 #ifdef DEBUG
+	static ps_packet test_packet_send();
+	static ps_packet test_packet_recv();
 	void show_packet() const;
 #endif
 

inc/p_util.h

@@ -0,0 +1,6 @@
+#ifndef __P_UTIL_H__
+#define __P_UTIL_H__
+
+#define UNUSED(x) (void)(x)
+
+#endif

src/main.cpp

@@ -1,5 +1,6 @@
 #include "p_serial_bus.h"
 #include "p_serial_packet.h"
+#include "p_util.h"
 #include <iostream>
 #include <signal.h>
 #include <stdio.h>
@@ -7,6 +8,10 @@
 #include <string>
 #include <zmq.hpp>
 
+#ifdef RUN_TESTS
+#include "tests.h"
+#endif
+
 #define SERIAL_DEV ("/dev/ttyAMA1")
 #define ZMQ_IPC_ADDR ("ipc:///penguinator/pubsub")
 
@@ -17,6 +22,7 @@ bool b_quit = false;
 
 void sigint_handler(int param)
 {
+	(void)param;
 	b_quit = true;
 	std::cout << "Exiting..." << std::endl;
 	return;
@@ -24,6 +30,10 @@ void sigint_handler(int param)
 
 int main()
 {
+#ifdef RUN_TESTS
+	run_tests();
+	return 0;
+#endif
 	signal(SIGINT, sigint_handler);
 
 	ps_bus sbus(SERIAL_DEV);
@@ -39,7 +49,7 @@ int main()
 		int32_t y = 0;
 		try
 		{
-			if (sub.recv(&msg_recv, 0))
+			if (sub.recv(msg_recv, zmq::recv_flags(0)))
 			{
 				uint8_t xy[64];
 				memset(xy, 0, 64);

src/p_serial_bus.cpp

@@ -1,4 +1,5 @@
 #include "p_serial_bus.h"
+#include "p_util.h"
 #include <cstring>
 #include <errno.h>
 #include <fcntl.h>
@@ -33,7 +34,7 @@ ps_bus::ps_bus(const char* dev)
 	/* Set logical level for RTS pin equal to 1 after sending: */
 	// rs485conf.flags |= SER_RS485_RTS_AFTER_SEND;
 	/* or, set logical level for RTS pin equal to 0 after sending: */
-	rs485conf.flags &= ~(SER_RS485_RTS_AFTER_SEND);
+	rs485conf.flags &= (uint32_t) ~(SER_RS485_RTS_AFTER_SEND);
 
 	// /* Set rts delay before send, if needed: */
 	// rs485conf.delay_rts_before_send = ...;
@@ -46,18 +47,18 @@ ps_bus::ps_bus(const char* dev)
 
 	memset(&toptions, 0, sizeof(toptions));
 	tcgetattr(dev_fd, &toptions);
-	toptions.c_cflag &= ~(CSIZE | PARENB);
+	toptions.c_cflag &= (uint32_t) ~(CSIZE | PARENB);
 	// enable receiver
 	toptions.c_cflag |= CREAD;
 	// 8 data bit
 	toptions.c_cflag |= CS8;
 
 	// Ignore framing errors and parity errors
-	toptions.c_lflag &= ~(ICANON);
-	toptions.c_lflag &= ~(ECHO);
-	toptions.c_lflag &= ~(ECHOE);
+	toptions.c_lflag &= (uint32_t) ~(ICANON);
+	toptions.c_lflag &= (uint32_t) ~(ECHO);
+	toptions.c_lflag &= (uint32_t) ~(ECHOE);
 	// disable signal chars
-	toptions.c_lflag &= ~(ISIG);
+	toptions.c_lflag &= (uint32_t) ~(ISIG);
 	// set baud rate
 	cfsetspeed(&toptions, B115200);
 	// flush cache
@@ -84,7 +85,7 @@ bool ps_bus::send_pkt(const ps_packet* const pkt)
 {
 	bool ret = true;
 	pkt->show_packet();
-	if (write(dev_fd, pkt->data, pkt->msg_len) != pkt->msg_len)
+	if (write(dev_fd, pkt->data, pkt->msg_len) != (int)pkt->msg_len)
 	{
 		ret = false;
 	}
@@ -94,6 +95,7 @@ bool ps_bus::send_pkt(const ps_packet* const pkt)
 }
 bool ps_bus::recv_pkt(ps_packet* const pkt)
 {
+	UNUSED(pkt);
 	bool ret = true;
 	return ret;
 }

src/p_serial_packet.cpp

@@ -1,11 +1,21 @@
 #include "p_serial_packet.h"
+#include "p_util.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* _msg, size_t _msg_len)
+{
+	memset(data, 0, MAX_MESSAGE_LEN);
+	memcpy(data, _msg, _msg_len);
+	src_addr = &data[1];
+	dest_addr = &data[2];
+	frame_data_len = &data[3];
+	frame_data = &data[4];
+	msg_len = _msg_len;
+	checksum = nullptr;
+}
 
 ps_packet::ps_packet(uint8_t _src_addr, uint8_t _dest_addr,
 					 uint8_t* _frame_data, uint8_t _frame_data_len)
@@ -56,7 +66,7 @@ bool ps_packet::decode()
 			ret = false;
 			break;
 		}
-
+		checksum = &data[msg_len - 1];
 		if (!validate())
 		{
 			ret = false;
@@ -70,12 +80,14 @@ bool ps_packet::decode()
 // done after receiving a packet and destuffing it
 bool ps_packet::validate()
 {
-	bool ret = true;
-	do
+	uint8_t csum = calc_checksum();
+	if (csum != *checksum)
 	{
-
-	} while (0);
-	return ret;
+		printf("Bad checksum check\nCalculated: 0x%02x\nActual: 0x%02x\n", csum,
+			   *checksum);
+		return false;
+	}
+	return true;
 }
 
 uint8_t ps_packet::calc_checksum()
@@ -119,7 +131,30 @@ bool ps_packet::stuff_bytes()
 
 bool ps_packet::destuff_bytes()
 {
-	return false;
+	bool ret = true;
+
+	uint8_t _checksum = data[msg_len - 1];
+	int old_ind = 0;
+	int new_ind = 0;
+	uint8_t temp_buffer[MAX_FRAME_DATA_LEN] = {0};
+	memcpy(temp_buffer, frame_data, MAX_FRAME_DATA_LEN);
+	memset(frame_data, 0, MAX_FRAME_DATA_LEN);
+	for (; new_ind < *frame_data_len; new_ind++)
+	{
+		if (temp_buffer[old_ind] == 0x7D)
+		{
+			msg_len--;
+			frame_data[new_ind] = temp_buffer[++old_ind] ^ 0x20;
+		}
+		else
+		{
+			frame_data[new_ind] = temp_buffer[old_ind];
+		}
+		old_ind++;
+	}
+	checksum = &data[msg_len - 1];
+	*checksum = _checksum;
+	return ret;
 }
 
 ps_packet ps_packet::from_xy(int32_t x, int32_t y)
@@ -135,7 +170,7 @@ 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()
+ps_packet ps_packet::test_packet_send()
 {
 	uint8_t frame_data[16] = {0xFF, 0x01, 0x02, 0x03, 0x04, 0x7D, 0x7D, 0x07,
 							  0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0F, 0x7E};
@@ -143,6 +178,16 @@ ps_packet ps_packet::test_packet()
 	return ps_packet(MASTER_ADDR, 0xFF, frame_data, 16);
 }
 
+ps_packet ps_packet::test_packet_recv()
+{
+	uint8_t frame_data[24] = {0x7E, 0xFF, MASTER_ADDR, 0x10, 0xFF, 0x01,
+							  0x02, 0x03, 0x04,		   0x7D, 0x5D, 0x7D,
+							  0x5D, 0x07, 0x08,		   0x09, 0x0A, 0x0B,
+							  0x0C, 0x0D, 0x0F,		   0x7D, 0x5E, 0x29};
+
+	return ps_packet(frame_data, 24);
+}
+
 #ifdef DEBUG
 void ps_packet::show_packet() const
 {
@@ -157,6 +202,6 @@ void ps_packet::show_packet() const
 	}
 	printf("\n");
 	printf("Message Length: %lu\n", msg_len);
-	printf("Checksum: %0x02x\n", *checksum);
+	printf("Checksum: 0x%02x\n", *checksum);
 }
 #endif

tests/tests.cpp

@@ -0,0 +1,24 @@
+#include "tests.h"
+#include "p_serial_packet.h"
+
+void run_tests()
+{
+
+	// tests
+	ps_packet test_pkt_send = ps_packet::test_packet_send();
+	test_pkt_send.encode();
+	test_pkt_send.show_packet();
+	printf("\n\n");
+	bool decode_ret = test_pkt_send.decode();
+	printf("Decode was %s\n", decode_ret ? "Successful" : "Unsuccessful");
+	test_pkt_send.show_packet();
+	printf("\n\n");
+
+	ps_packet test_pkt_recv = ps_packet::test_packet_recv();
+	bool decode_ret_recv = test_pkt_recv.decode();
+	printf("Decode was %s\n", decode_ret_recv ? "Successful" : "Unsuccessful");
+	test_pkt_recv.show_packet();
+	printf("\n\n");
+	test_pkt_recv.encode();
+	test_pkt_recv.show_packet();
+}

tests/tests.h

@@ -0,0 +1,6 @@
+#ifndef __TESTS_H__
+#define __TESTS_H__
+
+void run_tests();
+
+#endif