throw error on noisy serial channel

7 files changed, 53 insertions, 9 deletions

diff --git a/robot/- b/robot/-
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/robot/-
diff --git a/robot/hypervisor.c b/robot/hypervisor.c
index 0baa406..1fd3ac2 100644
--- a/robot/hypervisor.c
+++ b/robot/hypervisor.c
@@ -6,12 +6,13 @@
 #include "orangutan_shim.h"
 #include "sercomm.h"
 
-uint64_t g_w2_hypervisor_cycles				  = 0;
-uint64_t g_w2_hypervisor_uptime_ms			  = 0;
-unsigned long g_w2_hypervisor_ema_sercomm_ms  = 0;
-unsigned long g_w2_hypervisor_ema_errcatch_ms = 0;
-unsigned long g_w2_hypervisor_ema_io_ms		  = 0;
-unsigned long g_w2_hypervisor_ema_mode_ms	  = 0;
+uint64_t g_w2_hypervisor_cycles							   = 0;
+uint64_t g_w2_hypervisor_uptime_ms						   = 0;
+unsigned long g_w2_hypervisor_ema_sercomm_ms			   = 0;
+unsigned long g_w2_hypervisor_ema_errcatch_ms			   = 0;
+unsigned long g_w2_hypervisor_ema_io_ms					   = 0;
+unsigned long g_w2_hypervisor_ema_mode_ms				   = 0;
+uint64_t g_w2_hypervisor_timers[W2_HYPERVISOR_TIMER_COUNT] = {0};
 
 void w2_hypervisor_main() {
 #ifdef W2_SIM
@@ -51,3 +52,11 @@ void w2_hypervisor_main() {
 
 	g_w2_hypervisor_cycles++;
 }
+
+void w2_hypervisor_time_start(uint8_t label) {
+	g_w2_hypervisor_timers[label] = g_w2_hypervisor_uptime_ms;
+}
+
+uint64_t w2_hypervisor_time_end(uint8_t label) {
+	return g_w2_hypervisor_uptime_ms - g_w2_hypervisor_timers[label];
+}
diff --git a/robot/hypervisor.h b/robot/hypervisor.h
index b5dc0ab..589d324 100644
--- a/robot/hypervisor.h
+++ b/robot/hypervisor.h
@@ -4,6 +4,9 @@
 
 #include <stdint.h>
 
+/** amount of parallel timers */
+#define W2_HYPERVISOR_TIMER_COUNT (1)
+
 extern uint64_t g_w2_hypervisor_cycles;
 extern uint64_t g_w2_hypervisor_uptime_ms;
 
@@ -18,3 +21,8 @@ extern unsigned long g_w2_hypervisor_ema_mode_ms;
  * stores global variables and controls when other modules run
  */
 void w2_hypervisor_main();
+
+/** start timer with label `label` */
+void w2_hypervisor_time_start(uint8_t label);
+/** stop timer with label `label` */
+uint64_t w2_hypervisor_time_end(uint8_t label);
diff --git a/robot/readme.md b/robot/readme.md
index 0c8f626..168be02 100644
--- a/robot/readme.md
+++ b/robot/readme.md
@@ -43,7 +43,7 @@ organizational and form more of a software 'skeleton', while the 'maze' and
                                                           Maze ─┤
                                                      Warehouse ─┤
                                                 Emergency stop ─┤
-                      *modes* ->                  Line finding ─┤
+                        *logic modes* ->          Line finding ─┤
                                                 Charge station ─┤
                                                 Direct control ─┤
                                                      Wet floor ─┤
diff --git a/robot/sercomm.c b/robot/sercomm.c
index 2736030..83d6419 100644
--- a/robot/sercomm.c
+++ b/robot/sercomm.c
@@ -3,6 +3,7 @@
 
 #include "../shared/bin.h"
 #include "../shared/serial_parse.h"
+#include "errcatch.h"
 #include "hypervisor.h"
 #include "io.h"
 #include "mode_dirc.h"
@@ -51,6 +52,26 @@ void w2_sercomm_append_msg(w2_s_bin *data) {
 	g_w2_sercomm_index = next_index;
 }
 
+void w2_cmd_handler(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t data_length) {
+	w2_s_bin *copy				= w2_bin_s_alloc(data_length, data);
+	void (*handler)(w2_s_bin *) = g_w2_cmd_handlers[data[0]];
+
+	if (handler == NULL) {
+#ifdef W2_SIM
+		// TODO throw warning
+		simwarn("unknown serial message with code 0x%02x\n", data[0]);
+#endif
+		w2_errcatch_throw(W2_E_WARN_SERIAL_NOISY);
+	} else {
+#ifdef W2_SIM
+		w2_sim_print_serial(copy);
+#endif
+		handler(copy);
+	}
+
+	free(copy);
+}
+
 void w2_cmd_ping_rx(w2_s_bin *data) {
 	w2_s_cmd_ping_rx *message = malloc(w2_cmd_sizeof(data->data, data->bytes));
 	memcpy(message, data->data, data->bytes);
diff --git a/robot/sim.c b/robot/sim.c
index ba9aa4a..4efdc4d 100644
--- a/robot/sim.c
+++ b/robot/sim.c
@@ -105,7 +105,7 @@ void serial_send(char* message, unsigned int length) {
 }
 
 void serial_receive_ring(char* buffer, unsigned char size) {
-	simprintfunc("serial_receive_ring", "0x%016llx, %u", (uint64_t) buffer, size);
+	simprintfunc("serial_receive_ring", PTR_FMT ", %u", (uint64_t) buffer, size);
 }
 
 unsigned char serial_get_received_bytes() {
@@ -170,7 +170,7 @@ unsigned char get_single_debounced_button_press(unsigned char buttons) {
 }
 
 void qtr_read(unsigned int* sensor_values, unsigned char read_mode) {
-	simprintfunc("qtr_read", "0x%016llx, %s", (uint64_t) sensor_values, read_mode == QTR_EMITTERS_ON ? "QTR_EMITTERS_ON" : "???");
+	simprintfunc("qtr_read", PTR_FMT ", %s", (uint64_t) sensor_values, read_mode == QTR_EMITTERS_ON ? "QTR_EMITTERS_ON" : "???");
 	sensor_values[0] = 0;
 	sensor_values[1] = 0;
 	sensor_values[2] = 0;
diff --git a/robot/sim.h b/robot/sim.h
index 76b57f8..aa587cd 100644
--- a/robot/sim.h
+++ b/robot/sim.h
@@ -54,6 +54,12 @@ extern bool g_w2_sim_headless;
 		COL_CYN name COL_RST "(" COL_YEL fmt COL_RST ")\n", ##__VA_ARGS__); }
 #define simwarn(message, ...) if (DBG_ENABLE_SIMWARN) { simprintf(COL_YEL "[WARN] " COL_RST message, ##__VA_ARGS__); }
 #define siminfo(message, ...) if (DBG_ENABLE_SIMINFO) { simprintf(COL_MAG "[INFO] " COL_RST message, ##__VA_ARGS__); }
+#ifdef W2_HOST_LINUX
+#define PTR_FMT "0x%016lx"
+#endif
+#ifdef W2_HOST_WIN32
+#define PTR_FMT "0x%016llx"
+#endif
 
 #define BUTTON_A 0
 #define BUTTON_B 1