7 files changed, 26 insertions, 10 deletions

diff --git a/robot/hypervisor.c b/robot/hypervisor.c
index 4d46a12..0f754a3 100644
--- a/robot/hypervisor.c
+++ b/robot/hypervisor.c
@@ -5,6 +5,8 @@
 #include "orangutan_shim.h"
 #include "sercomm.h"
 
+uint64_t g_w2_hypervisor_cycles = 0;
+
 void w2_hypervisor_main() {
 #ifdef W2_SIM
 	w2_sim_cycle_begin();
@@ -26,6 +28,10 @@ void w2_hypervisor_main() {
 
 #ifdef W2_SIM
 	if (DBG_ENABLE_CYCLEINFO) siminfo("cycle end\n");
-	usleep(100e3);
+	if (!g_w2_sim_headless) usleep(100e3);
+
+	if (g_w2_sim_headless && DBG_MAX_CYCLES > -1 && g_w2_hypervisor_cycles > DBG_MAX_CYCLES) exit(0);
 #endif
+
+	g_w2_hypervisor_cycles++;
 }
diff --git a/robot/hypervisor.h b/robot/hypervisor.h
index 0e73259..35fa64c 100644
--- a/robot/hypervisor.h
+++ b/robot/hypervisor.h
@@ -1,5 +1,9 @@
 #pragma once
 
+#include <stdint.h>
+
+extern uint64_t g_w2_hypervisor_cycles;
+
 /**
  * backbone of all other modules
  *
diff --git a/robot/readme.md b/robot/readme.md
index 4a7aca3..2de14db 100644
--- a/robot/readme.md
+++ b/robot/readme.md
@@ -110,6 +110,8 @@ global todo:
   module (for last ping time measurement)
 - [ ] calibrate (line-detecting) light sensors in setup.c, or manually by
   placing the robot and pressing a button (maybe make this a seperate mode)
+- [ ] create labeled timer functions like nodejs `console.time()` and
+  `console.timeEnd()` (use for serial read timeout constraint)
 
 ### hypervisor
 
diff --git a/robot/sercomm.c b/robot/sercomm.c
index 7072f9e..f4b7eb5 100644
--- a/robot/sercomm.c
+++ b/robot/sercomm.c
@@ -2,6 +2,7 @@
 #include <string.h>
 
 #include "../shared/bin.h"
+#include "../shared/serial_parse.h"
 #include "orangutan_shim.h"
 #include "sercomm.h"
 
@@ -27,11 +28,9 @@ void w2_sercomm_main() {
 		g_w2_sercomm_offset = (g_w2_sercomm_offset + 1) % W2_SERCOMM_BUFFER_SIZE;
 	}
 
+	// read and parse data
 	while (serial_get_received_bytes() != g_w2_serial_buffer_index) {
-		uint8_t byte = g_w2_serial_buffer[g_w2_serial_buffer_index];
-#ifdef W2_SIM
-		simprintf("serial byte: %02x\n", byte);
-#endif
+		w2_serial_parse(g_w2_serial_buffer[g_w2_serial_buffer_index]);
 		g_w2_serial_buffer_index = (g_w2_serial_buffer_index + 1) % W2_SERIAL_READ_BUFFER_SIZE;
 	}
 }
@@ -43,10 +42,7 @@ void w2_sercomm_append_msg(w2_s_bin *data) {
 	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->data, data->data, data->bytes);
-	data_copy->bytes						= data->bytes;
-	g_w2_sercomm_buffer[g_w2_sercomm_index] = data_copy;
+	g_w2_sercomm_buffer[g_w2_sercomm_index] = w2_bin_s_alloc(data->bytes, data->data);
 	if (g_w2_sercomm_buffer_full) return;
 	g_w2_sercomm_index = next_index;
 }
diff --git a/robot/sim.c b/robot/sim.c
index b061c9a..8af672e 100644
--- a/robot/sim.c
+++ b/robot/sim.c
@@ -52,6 +52,12 @@ void serial_set_baud_rate(unsigned int rate) {
 }
 
 void serial_send(char* message, unsigned int length) {
+	if (g_w2_sim_headless) {
+		for (unsigned int byte = 0; byte < length; byte++)
+			putc(message[byte] & 0xff, stdout);
+		return;
+	}
+	if (!DBG_ENABLE_PRINTFUNC) return;
 	simprintfunc("serial_send", "<see below>, %u", length);
 	unsigned int bytes = 0;
 	simprintf("");
diff --git a/robot/sim.h b/robot/sim.h
index 501552e..336592b 100644
--- a/robot/sim.h
+++ b/robot/sim.h
@@ -8,10 +8,11 @@
 extern bool g_w2_sim_headless;
 
 // 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)
+#define DBG_MAX_CYCLES (10)
 
 // debug print options
 #define DBG_BYTES_PER_LINE 16
diff --git a/robot/tests/padded_info.bin b/robot/tests/padded_info.bin
new file mode 100644
index 0000000..2ff1d8f
--- /dev/null
+++ b/robot/tests/padded_info.bin
@@ -0,0 +1 @@
+=ãÿ=íÿÿI89ÿ
\ No newline at end of file