error handling done + better simulation / debug messaging

9 files changed, 92 insertions, 49 deletions

diff --git a/robot/consts.h b/robot/consts.h
index 93cafa5..c7bbc3f 100644
--- a/robot/consts.h
+++ b/robot/consts.h
@@ -32,4 +32,5 @@ enum w2_e_errorcodes {
 	W2_ERR_BATTERY_LOW		 = 0x00 | W2_ERR_TYPE_WARN,
 	W2_ERR_OBSTACLE_DETECTED = 0x01 | W2_ERR_TYPE_WARN,
 	W2_ERR_CYCLE_EXPIRED	 = 0x02 | W2_ERR_TYPE_WARN,
+	W2_ERR_UNCAUGHT_ERROR    = 0x03 | W2_ERR_TYPE_WARN,
 };
diff --git a/robot/errcatch.c b/robot/errcatch.c
index b50ee4a..945aa96 100644
--- a/robot/errcatch.c
+++ b/robot/errcatch.c
@@ -1,12 +1,12 @@
+#include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
-#include <stdio.h>
 
+#include "consts.h"
 #include "errcatch.h"
+#include "halt.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;
@@ -14,7 +14,9 @@ uint8_t g_w2_error_offset						   = 0;
 
 void w2_errcatch_main() {
 	while (g_w2_error_index != g_w2_error_offset) {
-		w2_errcatch_handle_error(g_w2_error_buffer[g_w2_error_offset]);
+		w2_s_error* error = &g_w2_error_buffer[g_w2_error_offset];
+		w2_errcatch_handle_error(error);
+		free(error);
 		g_w2_error_offset = (g_w2_error_offset + 1) % W2_ERROR_BUFFER_SIZE;
 	}
 }
@@ -34,13 +36,26 @@ void w2_errcatch_throw_msg(enum w2_e_errorcodes code, uint16_t length, const cha
 	g_w2_error_index					= (g_w2_error_index + 1) % W2_ERROR_BUFFER_SIZE;
 }
 
-void w2_errcatch_handle_error(w2_s_error error) {
-	uint8_t severity = error.code & W2_ERR_TYPE_MASK;
+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
+	// trigger emergency mode for critical errors
 	if ((severity ^ W2_ERR_TYPE_CRIT) == 0) g_w2_current_mode = &w2_mode_halt;
 
-	//TODO: forward error to sercomm
+	// TODO: handle more error types
+	switch (error->code) {
+		case W2_ERR_UNCAUGHT_ERROR: {
+			break;
+		}
+		default: {
+			w2_errcatch_throw(W2_ERR_UNCAUGHT_ERROR);
+			#ifdef W2_SIM
+			simwarn("Uncaught/unhandled error found with code 0x%02x", error->code);
+			#endif
+		}
+	}
+
+	// TODO: forward error to sercomm
 
 	return;
 }
diff --git a/robot/errcatch.h b/robot/errcatch.h
index 7ced980..5afe5b3 100644
--- a/robot/errcatch.h
+++ b/robot/errcatch.h
@@ -27,7 +27,7 @@ extern uint8_t g_w2_error_offset;
 void w2_errcatch_main();
 
 /** handle error */
-void w2_errcatch_handle_error(w2_s_error error);
+void w2_errcatch_handle_error(w2_s_error* error);
 
 /** append error to error buffer */
 void w2_errcatch_throw(enum w2_e_errorcodes code);
@@ -41,4 +41,3 @@ void w2_errcatch_throw_msg(enum w2_e_errorcodes code, uint16_t length, const cha
  * 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 0350163..2e263ce 100644
--- a/robot/hypervisor.c
+++ b/robot/hypervisor.c
@@ -1,15 +1,10 @@
-#include <stdlib.h>
-#include <time.h>
-#include <stdio.h>
-#include <unistd.h>
-
 #include "consts.h"
 #include "errcatch.h"
 #include "hypervisor.h"
 #include "io.h"
 #include "modes.h"
-#include "sercomm.h"
 #include "orangutan_shim.h"
+#include "sercomm.h"
 
 void w2_hypervisor_main() {
 	time_reset();
@@ -23,20 +18,14 @@ void w2_hypervisor_main() {
 	w2_modes_main();
 	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);
+	#ifdef W2_SIM
+	siminfo("sercomm:  %lums\n", sercomm_time);
+	siminfo("errcatch: %lums\n", errcatch_time);
+	siminfo("io:       %lums\n", io_time);
+	siminfo("mode:     %lums\n", mode_time);
 
 	usleep(100e3);
+	#endif
 
 	if (mode_time > W2_MAX_MODULE_CYCLE_MS) w2_errcatch_throw(W2_ERR_CYCLE_EXPIRED);
 }
diff --git a/robot/makefile b/robot/makefile
index 1036a26..6f50519 100644
--- a/robot/makefile
+++ b/robot/makefile
@@ -4,26 +4,30 @@ AVRDUDE_DEVICE = m328p
 DEVICE ?= atmega168
 MCU ?= atmega168
 AVRDUDE_DEVICE ?= m168
-SIM = true
 
 PORT ?= /dev/ttyACM0
 
 CFLAGS=-g -Wall $(DEVICE_SPECIFIC_CFLAGS) -Os
 LDFLAGS=-Wl,-gc-sections -Wl,-relax
 
+SOURCES := $(filter-out sim.c, $(wildcard *.c))
+HEADERS := $(filter-out sim.h, $(wildcard *.h))
+
+# simulation
+# SIM = true
 CFLAGS += $(if $(SIM), -DW2_SIM, -mcall-prologues -mmcu=$(MCU))
 LDFLAGS += $(if $(SIM), , -lpololu_$(DEVICE))
+PREFIX := $(if $(SIM), , avr-)
+SOURCES += $(if $(SIM), sim.c, )
+HEADERS += $(if $(SIM), sim.h, )
 
-SOURCES := $(wildcard *.c)
-HEADERS := $(wildcard *.h)
 OBJECTS := $(patsubst %.c,%.o, $(SOURCES))
 
-PREFIX := $(if $(SIM), , avr-)
-
 AVRDUDE=avrdude
 CC=$(PREFIX)gcc
 OBJ2HEX=$(PREFIX)objcopy
 
+# debug build info string
 BUILD_STR=$(shell git update-index -q --refresh; git describe --tags --dirty='*' --broken='x' | cut -c1-20)
 CFLAGS += -DW2_BUILD_STR="$(BUILD_STR)"
 
@@ -46,8 +50,8 @@ flash: out.hex
 	$(AVRDUDE) -p $(AVRDUDE_DEVICE) -c avrisp2 -P $(PORT) -U flash:w:out.hex
 
 format:
-	SOURCES := $(filter-out sim.c, $(SOURCES))
-	HEADERS := $(filter-out sim.h, $(HEADERS))
+	$(eval SOURCES := $(filter-out sim.c, $(SOURCES)))
+	$(eval HEADERS := $(filter-out sim.h, $(HEADERS)))
 	clang-format -i $(SOURCES) $(HEADERS)
 	clang-tidy --fix-errors $(SOURCES) $(HEADERS)
 
diff --git a/robot/readme.md b/robot/readme.md
index 1ffdb65..5e9990e 100644
--- a/robot/readme.md
+++ b/robot/readme.md
@@ -124,7 +124,7 @@ errors, and (b) handles errors accordingly.
   - [x] error code
   - [x] message length
   - [x] message contents
-- [ ] create a global error ring buffer with an appropriate size that holds
+- [x] create a global error ring buffer with an appropriate size that holds
   error messages
 - [ ] handle errors in the error buffer, referencing the functional
   specification for details on what the robot should do to resolve each kind of
diff --git a/robot/setup.c b/robot/setup.c
index 1174570..c74cca9 100644
--- a/robot/setup.c
+++ b/robot/setup.c
@@ -1,9 +1,9 @@
 #include <stdlib.h>
 
-#include "orangutan_shim.h"
 #include "consts.h"
 #include "halt.h"
 #include "modes.h"
+#include "orangutan_shim.h"
 #include "setup.h"
 
 void w2_setup_main() {
diff --git a/robot/sim.c b/robot/sim.c
index 3f7f686..6bd5838 100644
--- a/robot/sim.c
+++ b/robot/sim.c
@@ -7,13 +7,13 @@
 struct timespec reference_time; // NOLINT
 
 void time_reset() {
-	// printf("SIM: time_reset()\n");
+	simprintfunc("time_reset", "");
 	clock_gettime(CLOCK_MONOTONIC, &reference_time);
 	return;
 }
 
 unsigned long get_ms() {
-	// printf("SIM: get_ms()\n");
+	simprintfunc("get_ms", "");
 	struct timespec elapsed;
 	clock_gettime(CLOCK_MONOTONIC, &elapsed);
 	return ((elapsed.tv_sec * 1000) + (elapsed.tv_nsec / 1000000)) -
@@ -21,35 +21,44 @@ unsigned long get_ms() {
 }
 
 void red_led(unsigned char on) {
-	printf("SIM: red_led(%i)\n", on);
+	simprintfunc("red_led", "%i", on);
 	return;
 }
 
 void green_led(unsigned char on) {
-	printf("SIM: green_led(%i)\n", on);
+	simprintfunc("green_led", "%i", on);
 	return;
 }
 
 void clear() {
-	printf("SIM: clear()\n");
+	simprintfunc("clear", "");
 	return;
 }
 
 void play(const char* melody) {
-	printf("SIM: play(\"%s\")\n", melody);
+	simprintfunc("play", "\"%s\"", melody);
 	return;
 }
 
 void serial_set_baud_rate(unsigned int rate) {
-	printf("SIM: serial_set_baud_rate(%u)\n", rate);
+	simprintfunc("serial_set_baud_rate", "%u", 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);
+	simprintfunc("serial_send", "<see below>, %u", length);
+	unsigned int bytes = 0;
+	simprintf("");
+	for (unsigned int byte = 0; byte < length; byte++) {
+		if (bytes > DBG_BYTES_PER_LINE) {
+			bytes = 0;
+			printf("\n");
+			simprintf("");
+		}
+		printf("%02x ", message[byte]);
+		bytes++;
+	}
+	printf("\n");
 	return;
 }
 
diff --git a/robot/sim.h b/robot/sim.h
index 08faee5..15a1b4b 100644
--- a/robot/sim.h
+++ b/robot/sim.h
@@ -1,9 +1,35 @@
 #pragma once
 
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+
+// debug print options
+#define DBG_BYTES_PER_LINE 16
+
+// debug colors
+#define COL_BLK "\e[0;30m"
+#define COL_RED "\e[0;31m"
+#define COL_GRN "\e[0;32m"
+#define COL_YEL "\e[0;33m"
+#define COL_BLU "\e[0;34m"
+#define COL_MAG "\e[0;35m"
+#define COL_CYN "\e[0;36m"
+#define COL_WHT "\e[0;37m"
+#define COL_RST "\e[0m"
+
+// debug stdout print macros
+#define simprintf(message, ...) printf(COL_RED "[SIM] " COL_RST message, ##__VA_ARGS__)
+#define simprint(message) simprintf(message "\n")
+#define simprintfunc(name, fmt, ...) simprintf(COL_BLU "[FUNC] " \
+		COL_CYN name COL_RST "(" COL_YEL fmt COL_RST ")\n", ##__VA_ARGS__)
+#define simwarn(message, ...) simprintf(COL_YEL "[WARN] " COL_RST message, ##__VA_ARGS__)
+#define siminfo(message, ...) simprintf(COL_MAG "[INFO] " COL_RST message, ##__VA_ARGS__)
+
 /**
  * simulates pololu library functions for local testing
+ * NOLINT is so clang-tidy doesn't correct function names
  */
-
 void time_reset(); // NOLINT
 unsigned long get_ms(); // NOLINT
 void red_led(unsigned char on); // NOLINT