debug error handling

7 files changed, 82 insertions, 21 deletions

diff --git a/robot/consts.h b/robot/consts.h
index fb67f26..93cafa5 100644
--- a/robot/consts.h
+++ b/robot/consts.h
@@ -13,6 +13,7 @@
 #define W2_ERR_TYPE_WARN (0b01 << 6)
 #define W2_ERR_TYPE_INFO (0b10 << 6)
 #define W2_ERR_TYPE_VERB (0b11 << 6)
+#define W2_ERR_TYPE_MASK (0b11 << 6)
 
 /**
  * enum storing all error codes
diff --git a/robot/errcatch.c b/robot/errcatch.c
index ec1ab74..b50ee4a 100644
--- a/robot/errcatch.c
+++ b/robot/errcatch.c
@@ -1,7 +1,12 @@
 #include <stdlib.h>
 #include <string.h>
+#include <stdio.h>
 
 #include "errcatch.h"
+#include "modes.h"
+#include "orangutan_shim.h"
+#include "consts.h"
+#include "halt.h"
 
 w2_s_error g_w2_error_buffer[W2_ERROR_BUFFER_SIZE] = {};
 uint8_t g_w2_error_index						   = 0;
@@ -9,8 +14,7 @@ uint8_t g_w2_error_offset						   = 0;
 
 void w2_errcatch_main() {
 	while (g_w2_error_index != g_w2_error_offset) {
-		// handle g_w2_error_buffer[g_w2_error_offset];
-
+		w2_errcatch_handle_error(g_w2_error_buffer[g_w2_error_offset]);
 		g_w2_error_offset = (g_w2_error_offset + 1) % W2_ERROR_BUFFER_SIZE;
 	}
 }
@@ -23,10 +27,20 @@ w2_s_error *w2_alloc_error(enum w2_e_errorcodes code, uint16_t length, const cha
 	return error;
 }
 
+void w2_errcatch_throw(enum w2_e_errorcodes code) { w2_errcatch_throw_msg(code, 0, ""); }
 void w2_errcatch_throw_msg(enum w2_e_errorcodes code, uint16_t length, const char *message) {
 	w2_s_error error					= *w2_alloc_error(code, length, message);
 	g_w2_error_buffer[g_w2_error_index] = error;
-	g_w2_error_index					= (g_w2_error_index)&W2_ERROR_BUFFER_SIZE;
+	g_w2_error_index					= (g_w2_error_index + 1) % W2_ERROR_BUFFER_SIZE;
 }
 
-void w2_errcatch_throw(enum w2_e_errorcodes code) { w2_errcatch_throw_msg(code, 0, ""); }
+void w2_errcatch_handle_error(w2_s_error error) {
+	uint8_t severity = error.code & W2_ERR_TYPE_MASK;
+
+	// go into emergency mode for critical errors
+	if ((severity ^ W2_ERR_TYPE_CRIT) == 0) g_w2_current_mode = &w2_mode_halt;
+
+	//TODO: forward error to sercomm
+
+	return;
+}
diff --git a/robot/errcatch.h b/robot/errcatch.h
index 3bdc330..7ced980 100644
--- a/robot/errcatch.h
+++ b/robot/errcatch.h
@@ -4,15 +4,6 @@
 
 #include "consts.h"
 
-/** error-handler module main */
-void w2_errcatch_main();
-
-/** append error to error buffer */
-void w2_errcatch_throw(enum w2_e_errorcodes code);
-
-/** append error to error buffer (with debug message) */
-void w2_errcatch_throw_msg(enum w2_e_errorcodes code, uint16_t length, const char *message);
-
 /**
  * error struct
  *
@@ -32,9 +23,22 @@ extern uint8_t g_w2_error_index;
 /** stores start of ring buffer */
 extern uint8_t g_w2_error_offset;
 
+/** error-handler module main */
+void w2_errcatch_main();
+
+/** handle error */
+void w2_errcatch_handle_error(w2_s_error error);
+
+/** append error to error buffer */
+void w2_errcatch_throw(enum w2_e_errorcodes code);
+
+/** append error to error buffer (with debug message) */
+void w2_errcatch_throw_msg(enum w2_e_errorcodes code, uint16_t length, const char *message);
+
 /**
  * allocate and initialize error struct
  *
  * TODO: doesn't handle null pointers from calloc
  */
 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 8c22316..0350163 100644
--- a/robot/hypervisor.c
+++ b/robot/hypervisor.c
@@ -1,3 +1,8 @@
+#include <stdlib.h>
+#include <time.h>
+#include <stdio.h>
+#include <unistd.h>
+
 #include "consts.h"
 #include "errcatch.h"
 #include "hypervisor.h"
@@ -7,12 +12,31 @@
 #include "orangutan_shim.h"
 
 void w2_hypervisor_main() {
+	time_reset();
+
 	w2_sercomm_main();
+	unsigned long sercomm_time = get_ms();
 	w2_errcatch_main();
+	unsigned long errcatch_time = get_ms() - sercomm_time;
 	w2_io_main();
-
-	time_reset();
+	unsigned long io_time = get_ms() - errcatch_time;
 	w2_modes_main();
-	unsigned long elapsed_ms = get_ms();
-	if (elapsed_ms > W2_MAX_MODULE_CYCLE_MS) w2_errcatch_throw(W2_ERR_CYCLE_EXPIRED);
+	unsigned long mode_time = get_ms() - io_time;
+
+	char* message = malloc(80);
+	sprintf(message, "sercomm: %lums  ", sercomm_time);
+	serial_send(message, 80);
+	sprintf(message, "errcatch: %lums ", errcatch_time);
+	serial_send(message, 80);
+	sprintf(message, "io: %lums       ", io_time);
+	serial_send(message, 80);
+	sprintf(message, "mode: %lums     ", mode_time);
+	serial_send(message, 80);
+	sprintf(message, "                ");
+	serial_send(message, 80);
+	free(message);
+
+	usleep(100e3);
+
+	if (mode_time > W2_MAX_MODULE_CYCLE_MS) w2_errcatch_throw(W2_ERR_CYCLE_EXPIRED);
 }
diff --git a/robot/makefile b/robot/makefile
index fb30b7e..1036a26 100644
--- a/robot/makefile
+++ b/robot/makefile
@@ -4,7 +4,7 @@ AVRDUDE_DEVICE = m328p
 DEVICE ?= atmega168
 MCU ?= atmega168
 AVRDUDE_DEVICE ?= m168
-SIM = 
+SIM = true
 
 PORT ?= /dev/ttyACM0
 
diff --git a/robot/sim.c b/robot/sim.c
index 8fc8d00..3f7f686 100644
--- a/robot/sim.c
+++ b/robot/sim.c
@@ -1,15 +1,23 @@
 #include <stdio.h>
+#include <time.h>
+#include <string.h>
 
 #include "sim.h"
 
+struct timespec reference_time; // NOLINT
+
 void time_reset() {
+	// printf("SIM: time_reset()\n");
+	clock_gettime(CLOCK_MONOTONIC, &reference_time);
 	return;
-	printf("SIM: time_reset()\n");
 }
 
 unsigned long get_ms() {
-	printf("SIM: get_ms()\n");
-	return 0;
+	// printf("SIM: get_ms()\n");
+	struct timespec elapsed;
+	clock_gettime(CLOCK_MONOTONIC, &elapsed);
+	return ((elapsed.tv_sec * 1000) + (elapsed.tv_nsec / 1000000)) -
+		((reference_time.tv_sec * 1000) + (reference_time.tv_nsec / 1000000));
 }
 
 void red_led(unsigned char on) {
@@ -36,3 +44,12 @@ void serial_set_baud_rate(unsigned int rate) {
 	printf("SIM: serial_set_baud_rate(%u)\n", rate);
 	return;
 }
+
+// TODO: hexdump binary data
+void serial_send(char* message, unsigned int length) {
+	char message_copy[length];
+	strncpy(message_copy, message, length);
+	printf("SIM: serial_send(\"%s\", %u)\n", message_copy, length);
+	return;
+}
+
diff --git a/robot/sim.h b/robot/sim.h
index a408bc6..08faee5 100644
--- a/robot/sim.h
+++ b/robot/sim.h
@@ -11,3 +11,4 @@ void green_led(unsigned char on); // NOLINT
 void clear(); // NOLINT
 void play(const char* melody); // NOLINT
 void serial_set_baud_rate(unsigned int rate); // NOLINT
+void serial_send(char* message, unsigned int length); // NOLINT