[WIP] serial communication (broken w/ segfault)

10 files changed, 168 insertions, 19 deletions

diff --git a/robot/bin.c b/robot/bin.c
index 4242edc..a2c91a4 100644
--- a/robot/bin.c
+++ b/robot/bin.c
@@ -1,3 +1,5 @@
+#include <stdlib.h>
+
 #include "bin.h"
 
 #define W2_ENDIAN_LITTLE (1)
@@ -12,8 +14,40 @@
 #define _BYTE_2 ((uint32_t)(0xff << (_SHIFT_2B)))
 #define _BYTE_3 ((uint32_t)(0xff << (_SHIFT_3B)))
 
+const uint8_t W2_STRUCT_T_SIZES[] = {sizeof(uint8_t), sizeof(uint16_t), sizeof(uint32_t)};
+
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wshift-count-overflow"
+w2_s_bin *w2_bin_from_uint8_t(uint8_t data) {
+	size_t size	  = 1;
+	w2_s_bin *ret = malloc(sizeof(w2_s_bin) + sizeof(uint8_t) * size);
+	ret->bytes	  = size;
+	ret->data[0]  = data;
+	return ret;
+}
+
+w2_s_bin *w2_bin_from_uint16_t(uint16_t data) {
+	size_t size	  = 2;
+	w2_s_bin *ret = malloc(sizeof(w2_s_bin) + sizeof(uint8_t) * size);
+	data		  = w2_bin_hton16(data);
+	ret->bytes	  = size;
+	ret->data[0]  = (data & _BYTE_1) >> _SHIFT_1B;
+	ret->data[1]  = (data & _BYTE_0) >> _SHIFT_0B;
+	return ret;
+}
+
+w2_s_bin *w2_bin_from_uint32_t(uint32_t data) {
+	size_t size	  = 4;
+	w2_s_bin *ret = malloc(sizeof(w2_s_bin) + sizeof(uint8_t) * size);
+	data		  = w2_bin_hton32(data);
+	ret->bytes	  = size;
+	ret->data[0]  = (data & _BYTE_3) >> _SHIFT_3B;
+	ret->data[1]  = (data & _BYTE_2) >> _SHIFT_2B;
+	ret->data[2]  = (data & _BYTE_1) >> _SHIFT_1B;
+	ret->data[3]  = (data & _BYTE_0) >> _SHIFT_0B;
+	return ret;
+}
+
 uint32_t w2_bin_hton32(uint32_t h32) {
 	if (g_w2_endianness == W2_ENDIAN_BIG) return h32;
 	return ((h32 & _BYTE_0) << _SHIFT_3B) | ((h32 & _BYTE_1) << _SHIFT_1B) |
diff --git a/robot/bin.h b/robot/bin.h
index ccde132..1c9b951 100644
--- a/robot/bin.h
+++ b/robot/bin.h
@@ -1,19 +1,46 @@
 #pragma once
+/**
+ * helper file for binary data
+ *
+ * - fix endianness with functions inspired by UNIX arpa/inet.h
+ * - convert uint16_t and uint32_t to w2_s_bin
+ */
 
 #include <stdint.h>
 
+extern uint8_t g_w2_endianness;
+
+#define W2_T_UINT8_T (0)
+#define W2_T_UINT16_T (1)
+#define W2_T_UINT32_T (2)
+
+enum w2_e_struct_types {
+	W2_ST_UINT8_T,
+	W2_ST_UINT16_T,
+	W2_ST_UINT32_T,
+};
+
+extern const uint8_t W2_STRUCT_T_SIZES[];
+
 typedef struct {
 	uint16_t bytes;
 	uint8_t data[];
 } w2_s_bin;
 
-extern uint8_t g_w2_endianness;
+typedef struct {
+	enum w2_e_struct_types type;
+	uint16_t length;
+	const uint8_t *data;
+} w2_s_struct_property;
 
-/**
- * helper file for binary data
- *
- * - fix endianness with functions inspired by UNIX arpa/inet.h
- */
+typedef struct {
+	uint16_t length;
+	w2_s_struct_property *properties[];
+} w2_s_property_list;
+
+w2_s_bin *w2_bin_from_uint8_t(uint8_t data);
+w2_s_bin *w2_bin_from_uint16_t(uint16_t data);
+w2_s_bin *w2_bin_from_uint32_t(uint32_t data);
 
 /** convert 32-bit value from host endian to network (big-endian) */
 uint32_t w2_bin_hton32(uint32_t h32);
diff --git a/robot/errcatch.c b/robot/errcatch.c
index f66b2a1..82da820 100644
--- a/robot/errcatch.c
+++ b/robot/errcatch.c
@@ -1,4 +1,3 @@
-#include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
 
@@ -31,7 +30,7 @@ void w2_errcatch_main() {
 }
 
 w2_s_error *w2_alloc_error(enum w2_e_errorcodes code, uint16_t length, const char *message) {
-	w2_s_error *error = calloc(sizeof(w2_s_error) + length, 1);
+	w2_s_error *error = malloc(sizeof(w2_s_error) + length);
 	memcpy(error, &(w2_s_error const){.code = code, .message_length = length}, sizeof(w2_s_error));
 	strncpy(error->message, message, length);
 
diff --git a/robot/errcatch.h b/robot/errcatch.h
index c6edcfb..f0e25d9 100644
--- a/robot/errcatch.h
+++ b/robot/errcatch.h
@@ -39,6 +39,8 @@ enum w2_e_errorcodes {
 	W2_E_WARN_ERR_BUFFER_FULL = 0x04 | W2_E_TYPE_WARN,
 	/** line lost, trying to calibrate */
 	W2_E_WARN_LINE_LOST = 0x05 | W2_E_TYPE_WARN,
+	/** serial buffer full, gets thrown on next cycle */
+	W2_E_WARN_SERCOMM_BUFFER_FULL = 0x06 | W2_E_TYPE_WARN,
 };
 
 /**
@@ -75,6 +77,6 @@ void w2_errcatch_throw_msg(enum w2_e_errorcodes code, uint16_t length, const cha
 /**
  * allocate and initialize error struct
  *
- * TODO: doesn't handle null pointers from calloc
+ * TODO: doesn't handle null pointers from malloc
  */
 w2_s_error *w2_alloc_error(enum w2_e_errorcodes code, uint16_t length, const char *message);
diff --git a/robot/hypervisor.c b/robot/hypervisor.c
index c558de8..c08a24c 100644
--- a/robot/hypervisor.c
+++ b/robot/hypervisor.c
@@ -7,7 +7,7 @@
 
 void w2_hypervisor_main() {
 #ifdef W2_SIM
-	siminfo("cycle start\n");
+	if (DBG_ENABLE_CYCLEINFO) siminfo("cycle start\n");
 #endif
 
 	time_reset();
@@ -24,7 +24,7 @@ void w2_hypervisor_main() {
 	if (mode_time > W2_MAX_MODULE_CYCLE_MS) w2_errcatch_throw(W2_E_WARN_CYCLE_EXPIRED);
 
 #ifdef W2_SIM
-	siminfo("cycle end\n");
+	if (DBG_ENABLE_CYCLEINFO) siminfo("cycle end\n");
 	usleep(100e3);
 #endif
 }
diff --git a/robot/makefile b/robot/makefile
index ce57090..53010dd 100644
--- a/robot/makefile
+++ b/robot/makefile
@@ -14,7 +14,7 @@ SOURCES := $(filter-out sim.c, $(wildcard *.c))
 HEADERS := $(filter-out sim.h, $(wildcard *.h))
 
 # simulation
-# SIM = true
+SIM = true
 CFLAGS += $(if $(SIM), -DW2_SIM, -mcall-prologues -mmcu=$(MCU))
 LDFLAGS += $(if $(SIM), , -lpololu_$(DEVICE))
 PREFIX := $(if $(SIM), , avr-)
diff --git a/robot/sercomm.c b/robot/sercomm.c
index a0eed3a..e845b1a 100644
--- a/robot/sercomm.c
+++ b/robot/sercomm.c
@@ -1,3 +1,49 @@
+#include <stdlib.h>
+#include <string.h>
+
+#include "bin.h"
+#include "orangutan_shim.h"
 #include "sercomm.h"
 
-void w2_sercomm_main() {}
+w2_s_bin *g_w2_sercomm_buffer[W2_SERCOMM_BUFFER_SIZE] = {};
+uint8_t g_w2_sercomm_index							  = 0;
+uint8_t g_w2_sercomm_offset							  = 0;
+uint8_t g_w2_sercomm_buffer_full					  = 0;
+
+void w2_sercomm_main() {
+#ifdef W2_SIM
+	simprintfunc("w2_sercomm_main", "");
+#endif
+	// send data
+	while (g_w2_sercomm_index != g_w2_sercomm_offset) {
+#ifdef W2_SIM
+		simprint("line 0");
+		w2_s_bin *data = g_w2_sercomm_buffer[g_w2_sercomm_index];
+		simprint("line 1");
+
+		simprintf("bytes: %i\n", data->bytes);
+		char* data_cast = malloc(data->bytes);
+		simprint("line 2");
+		memcpy(data_cast, data->data, data->bytes);
+		simprint("line 3");
+		serial_send(data_cast, data->bytes);
+		simprint("line 4");
+		g_w2_sercomm_offset = (g_w2_sercomm_offset + 1) % W2_SERCOMM_BUFFER_SIZE;
+		simprint("line 5");
+#endif
+	}
+}
+
+void w2_sercomm_append_msg(w2_s_bin *data) {
+#ifdef W2_SIM
+	simprintfunc("w2_sercomm_append_msg", "");
+#endif
+	uint8_t next_index		 = (g_w2_sercomm_index + 1) % W2_SERCOMM_BUFFER_SIZE;
+	g_w2_sercomm_buffer_full = next_index == g_w2_sercomm_offset;
+	free(g_w2_sercomm_buffer[g_w2_sercomm_index]);
+	w2_s_bin *data_copy = malloc(sizeof(w2_s_bin) + sizeof(uint8_t) * data->bytes);
+	memcpy(&data_copy->bytes, data->data, data->bytes);
+	g_w2_sercomm_buffer[g_w2_sercomm_index] = data_copy;
+	if (g_w2_sercomm_buffer_full) return;
+	g_w2_sercomm_index = next_index;
+}
diff --git a/robot/sercomm.h b/robot/sercomm.h
index d960d0d..dd17af8 100644
--- a/robot/sercomm.h
+++ b/robot/sercomm.h
@@ -7,28 +7,42 @@
 #define W2_CMDDIR_TX (1)
 
 enum w2_e_serial_commands {
+	/** ping command */
 	W2_CMD_PING = 0x00,
+	/** exception command */
 	W2_CMD_EXPT = 0x02,
+	/** mode command */
 	W2_CMD_MODE = 0x04,
+	/** speed command */
 	W2_CMD_SPED = 0x06,
+	/** direct control command */
 	W2_CMD_DIRC = 0x08,
+	/** coordinate command */
 	W2_CMD_CORD = 0x0a,
+	/** backorder modify command */
 	W2_CMD_BOMD = 0x0c,
+	/** soft reset command */
 	W2_CMD_SRES = 0x0e,
+	/** map config command */
 	W2_CMD_MCFG = 0x10,
+	/** sensor data command */
 	W2_CMD_SENS = 0x12,
+	/** info command */
 	W2_CMD_INFO = 0x14,
+	/** display control command */
 	W2_CMD_DISP = 0x16,
+	/** play midi command */
 	W2_CMD_PLAY = 0x18,
+	/** control leds command */
 	W2_CMD_CLED = 0x1a,
 };
 
 /** sercomm ring buffer */
 extern w2_s_bin *g_w2_sercomm_buffer[W2_SERCOMM_BUFFER_SIZE];
 /** stores head of ring buffer */
-extern uint8_t g_w2_error_index;
+extern uint8_t g_w2_sercomm_index;
 /** stores start of ring buffer */
-extern uint8_t g_w2_error_offset;
+extern uint8_t g_w2_sercomm_offset;
 
 /**
  * serial pc-robot communication module
@@ -38,10 +52,9 @@ extern uint8_t g_w2_error_offset;
  */
 void w2_sercomm_main();
 
-void w2_sercomm_append_msg(w2_s_bin data);
+void w2_sercomm_append_msg(w2_s_bin *data);
 
-w2_s_bin w2_sercomm_rx_generic();
-w2_s_bin w2_sercomm_tx_generic();
+w2_s_bin w2_sercomm_struct_pack(w2_s_property_list generic_struct);
 
 w2_s_bin w2_sercomm_rx_ping();
 w2_s_bin w2_sercomm_tx_ping();
diff --git a/robot/setup.c b/robot/setup.c
index d040286..59a6404 100644
--- a/robot/setup.c
+++ b/robot/setup.c
@@ -27,4 +27,31 @@ void w2_setup_main() {
 
 	// indicate startup done
 	play("L50 c>c");
+
+#ifdef W2_SIM
+	#include "sercomm.h"
+	w2_s_bin *data = malloc(sizeof(w2_s_bin) + sizeof(uint8_t) * 20);
+	data->bytes = 20;
+	data->data[0] = 0x00;
+	data->data[1] = 0x01;
+	data->data[2] = 0x00;
+	data->data[3] = 0x01;
+	data->data[4] = 0x00;
+	data->data[5] = 0x01;
+	data->data[6] = 0x00;
+	data->data[7] = 0x01;
+	data->data[8] = 0xde;
+	data->data[9] = 0xad;
+	data->data[10] = 0xbe;
+	data->data[11] = 0xef;
+	data->data[12] = 0xff;
+	data->data[13] = 0xff;
+	data->data[14] = 0x00;
+	data->data[15] = 0x00;
+	data->data[16] = 0xff;
+	data->data[17] = 0xff;
+	data->data[18] = 0x00;
+	data->data[19] = 0x69;
+	w2_sercomm_append_msg(data);
+#endif
 }
diff --git a/robot/sim.h b/robot/sim.h
index 9c80ecf..c872dda 100644
--- a/robot/sim.h
+++ b/robot/sim.h
@@ -5,9 +5,10 @@
 #include <unistd.h>
 
 // debug fine-tuning
-#define DBG_ENABLE_PRINTFUNC (0)
+#define DBG_ENABLE_PRINTFUNC (1)
 #define DBG_ENABLE_SIMWARN (1)
 #define DBG_ENABLE_SIMINFO (1)
+#define DBG_ENABLE_CYCLEINFO (0)
 
 // debug print options
 #define DBG_BYTES_PER_LINE 16