throw error on noisy serial channel

10 files changed, 62 insertions, 36 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
diff --git a/shared/consts.h b/shared/consts.h
index 50b16b9..cdd96b3 100644
--- a/shared/consts.h
+++ b/shared/consts.h
@@ -12,21 +12,24 @@
 #warning "host operating system unknown"
 #endif
 
-/** max logic module execution time in milliseconds */
-#define W2_MAX_MODULE_CYCLE_MS (20)
 /** serial baud rate (bit/s) */
 #define W2_SERIAL_BAUD (9600)
+/** size of input (receive) buffer (in bytes) */
+#define W2_SERIAL_READ_BUFFER_SIZE (255)
+
 /** size of the error handling buffer (in errors, not bytes) */
 #define W2_ERROR_BUFFER_SIZE (16)
 /** size of the serial communication buffer (in messages, not bytes) */
 #define W2_SERCOMM_BUFFER_SIZE (16)
-/** size of input (receive) buffer (in bytes) */
-#define W2_SERIAL_READ_BUFFER_SIZE (255)
-/** exponential moving average new measurement weight (double 0-1) */
-#define W2_EMA_WEIGHT (0.10)
 /** size of mode history buffer */
 #define W2_MODE_HISTORY_BUFFER_SIZE (4)
 
+/** max logic module execution time in milliseconds */
+#define W2_MAX_MODULE_CYCLE_MS (20)
+
+/** exponential moving average new measurement weight (double 0-1) */
+#define W2_EMA_WEIGHT (0.10)
+
 /** front-facing distance sensor pinout */
 #define W2_FRONT_SENSOR_PIN 5
 /** battery voltage sensor pinout */
diff --git a/shared/errors.h b/shared/errors.h
index 4c9f7c8..344a506 100644
--- a/shared/errors.h
+++ b/shared/errors.h
@@ -45,8 +45,6 @@ typedef enum {
 	W2_E_WARN_SERCOMM_BUFFER_FULL = 0x06 | W2_E_TYPE_WARN,
 	/** semver minor version doesn't match */
 	W2_E_WARN_VERSION_INCOMPATIBLE = 0x07 | W2_E_TYPE_WARN,
-	/** serial byte took to long to receive */
-	W2_E_WARN_SERIAL_TIMEOUT = 0x08 | W2_E_TYPE_WARN,
 	/** unknown message encountered (noisy channel?) */
 	W2_E_WARN_SERIAL_NOISY = 0x09 | W2_E_TYPE_WARN,
 	/** mode history index out of bounds */
diff --git a/shared/protocol.c b/shared/protocol.c
index fcc9fa4..9be1d31 100644
--- a/shared/protocol.c
+++ b/shared/protocol.c
@@ -74,22 +74,3 @@ size_t w2_cmd_expt_tx_sizeof(w2_s_bin *data) {
 size_t w2_cmd_mcfg_rx_sizeof(w2_s_bin *data) {
 	return W2_DYN_MEMBER_SIZEOF(w2_s_cmd_mcfg_rx, 3, w2_s_cmd_mcfg_feature);
 }
-
-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
-	} else {
-#ifdef W2_SIM
-		w2_sim_print_serial(copy);
-#endif
-		handler(copy);
-	}
-
-	free(copy);
-}