forward errors to sercomm (implement expt command

4 files changed, 26 insertions, 9 deletions

diff --git a/protocol.md b/protocol.md
index 915fa65..7084d14 100644
--- a/protocol.md
+++ b/protocol.md
@@ -33,7 +33,7 @@ readability.
 |code|name|implemented|directions|full name|
 |--:|---|:-:|:-:|---|
 |`0x00`|[PING](#ping)|yes|`r <=> c`|<u>ping</u>
-|`0x02`|[EXPT](#expt)|no|`r --> c`|<u>ex</u>ce<u>pt</u>ion
+|`0x02`|[EXPT](#expt)|yes|`r --> c`|<u>ex</u>ce<u>pt</u>ion
 |`0x04`|[MODE](#mode)|no|`r <=> c`|<u>mode</u>
 |`0x06`|[SPED](#sped)|no|`r <-- c`|<u>spe</u>e<u>d</u>
 |`0x08`|[DIRC](#dirc)|no|`r <-- c`|<u>dir</u>ect <u>c</u>ontrol
@@ -294,10 +294,10 @@ requests robot info
 |-:|-|
 |`uint8_t`|opcode (`0x14 + 1`)|
 |`uint8_t[32]`|build string|
-|`uint8_t`|errcatch module cycle time (ms)|
-|`uint8_t`|io module cycle time (ms)|
-|`uint8_t`|sercomm module cycle time (ms)|
-|`uint8_t`|modes module cycle time (ms)|
+|`uint8_t`|exponential moving average errcatch module cycle time (ms)|
+|`uint8_t`|exponential moving average io module cycle time (ms)|
+|`uint8_t`|exponential moving average sercomm module cycle time (ms)|
+|`uint8_t`|exponential moving average modes module cycle time (ms)|
 |`uint32_t`|total robot uptime (s)|
 
 robot info response
diff --git a/robot/errcatch.c b/robot/errcatch.c
index 4bdbaef..99f1063 100644
--- a/robot/errcatch.c
+++ b/robot/errcatch.c
@@ -5,6 +5,7 @@
 #include "halt.h"
 #include "modes.h"
 #include "orangutan_shim.h"
+#include "sercomm.h"
 
 w2_s_error *g_w2_error_buffer[W2_ERROR_BUFFER_SIZE] = {};
 uint8_t g_w2_error_index							= 0;
@@ -66,7 +67,17 @@ void w2_errcatch_handle_error(w2_s_error *error) {
 		}
 	}
 
-	// TODO: forward error to sercomm
+	// forward error to sercomm
+	size_t msg_size		  = sizeof(w2_s_cmd_expt_tx) + sizeof(uint8_t) * error->message_length;
+	w2_s_cmd_expt_tx *msg = malloc(msg_size);
+	msg->opcode			  = W2_CMD_EXPT | W2_CMDDIR_TX;
+	msg->error			  = error->code;
+	msg->length			  = error->message_length;
+	memcpy(msg->message, error->message, error->message_length);
+	w2_s_bin *msg_bin = w2_bin_s_alloc(msg_size, (uint8_t *)msg);
+	w2_sercomm_append_msg(msg_bin);
+	free(msg);
+	free(msg_bin);
 
 	return;
 }
diff --git a/robot/sim.c b/robot/sim.c
index 0cde6a0..baf8a8a 100644
--- a/robot/sim.c
+++ b/robot/sim.c
@@ -9,6 +9,7 @@
 #include "../shared/consts.h"
 #include "../shared/protocol.h"
 #include "sercomm.h"
+#include "errcatch.h"
 
 struct timespec reference_time; // NOLINT
 bool g_w2_sim_headless = false;
@@ -74,10 +75,10 @@ void serial_send(char* message, unsigned int length) {
 			putc(message[byte] & 0xff, stdout);
 		return;
 	}
-	if (!DBG_ENABLE_PRINTFUNC) return;
 
-	simprintfunc("serial_send", "<see below>, %u", length);
+	if (DBG_ENABLE_PRINTFUNC) simprintfunc("serial_send", "<see below>, %u", length);
 
+	if (!DBG_ENABLE_SERIAL) return;
 	w2_s_bin *bin = w2_bin_s_alloc(length, (uint8_t*) message);
 	w2_sim_print_serial(bin);
 	free(bin);
@@ -103,6 +104,9 @@ void w2_sim_setup(int argc, char **argv) {
 	term.c_cc[VTIME] = 0;
 	term.c_cc[VMIN] = 0;
 	tcsetattr(STDIN_FILENO, 0, &term);
+
+	// debug error
+	// w2_errcatch_throw(W2_E_WARN_BATTERY_LOW);
 }
 
 void w2_sim_cycle_begin() {
diff --git a/robot/sim.h b/robot/sim.h
index 6e2498d..25cc713 100644
--- a/robot/sim.h
+++ b/robot/sim.h
@@ -10,10 +10,12 @@
 extern bool g_w2_sim_headless;
 
 // debug fine-tuning
-#define DBG_ENABLE_PRINTFUNC (1)
+#define DBG_ENABLE_PRINTFUNC (0)
 #define DBG_ENABLE_SIMWARN (1)
 #define DBG_ENABLE_SIMINFO (1)
 #define DBG_ENABLE_CYCLEINFO (0)
+#define DBG_ENABLE_SERIAL (1)
+
 #define DBG_MAX_CYCLES (10)
 
 // debug print options