implement mode history

8 files changed, 44 insertions, 12 deletions

diff --git a/robot/errcatch.c b/robot/errcatch.c
index e44acfd..8df90b8 100644
--- a/robot/errcatch.c
+++ b/robot/errcatch.c
@@ -51,7 +51,7 @@ void w2_errcatch_handle_error(w2_s_error *error) {
 	uint8_t severity = error->code & W2_E_TYPE_MASK;
 
 	// trigger emergency mode for critical errors
-	if ((severity ^ W2_E_TYPE_CRIT) == 0) w2_modes_switch(W2_M_HALT);
+	if ((severity ^ W2_E_TYPE_CRIT) == 0) w2_modes_call(W2_M_HALT);
 
 	// TODO: handle more error types
 	switch (error->code) {
diff --git a/robot/modes.c b/robot/modes.c
index 1f36703..8d3a099 100644
--- a/robot/modes.c
+++ b/robot/modes.c
@@ -1,15 +1,31 @@
+#include <stdbool.h>
+
+#include "../shared/util.h"
+#include "errcatch.h"
 #include "modes.h"
 #include "sercomm.h"
 
-void (*g_w2_current_mode)() = &w2_mode_halt;
+/** function pointer to current mode */
+// static void (*g_w2_current_mode)() = &w2_mode_halt;
+
+static void (*g_w2_mode_history[W2_MODE_HISTORY_BUFFER_SIZE])();
+static uint8_t g_w2_mode_history_index = 0;
+
+void w2_modes_main() { (*g_w2_mode_history[g_w2_mode_history_index])(); }
 
-void w2_modes_main() { (*g_w2_current_mode)(); }
+void w2_modes_switch(w2_e_mode new_mode, bool replace) {
+	int16_t next_history_index =
+		g_w2_mode_history_index + (new_mode == W2_M_PREV ? -1 : 1) * (replace - 1);
+	if (next_history_index == -1 || next_history_index == W2_MODE_HISTORY_BUFFER_SIZE - 1) {
+		next_history_index = W2_RANGE(0, next_history_index, W2_MODE_HISTORY_BUFFER_SIZE);
+		w2_errcatch_throw(W2_E_WARN_MODE_HISTORY_BUFFER_IOB);
+	}
 
-void w2_modes_switch(w2_e_mode new_mode) {
 	if (new_mode == W2_M_PREV) {
-		// TODO implement previous mode buffer
+		g_w2_mode_history_index = next_history_index;
 	} else {
-		g_w2_current_mode = W2_MODES[new_mode];
+		g_w2_mode_history_index					   = next_history_index;
+		g_w2_mode_history[g_w2_mode_history_index] = W2_MODES[new_mode];
 	}
 
 	// forward mode change to sercomm
@@ -22,3 +38,7 @@ void w2_modes_switch(w2_e_mode new_mode) {
 	free(msg);
 	free(msg_bin);
 }
+
+void w2_modes_call(w2_e_mode mode) { w2_modes_switch(mode, false); }
+
+void w2_modes_swap(w2_e_mode mode) { w2_modes_switch(mode, true); }
diff --git a/robot/modes.h b/robot/modes.h
index a9d96d8..3423a0f 100644
--- a/robot/modes.h
+++ b/robot/modes.h
@@ -1,5 +1,7 @@
 #pragma once
 
+#include "../shared/consts.h"
+
 #include "mode_chrg.h"
 #include "mode_dirc.h"
 #include "mode_grid.h"
@@ -9,9 +11,6 @@
 #include "mode_scal.h"
 #include "mode_spin.h"
 
-/** function pointer to current mode */
-extern void (*g_w2_current_mode)();
-
 /**
  * mode logic
  *
@@ -38,5 +37,7 @@ static const void(*const W2_MODES[]) = {
 	&w2_mode_chrg, &w2_mode_dirc, &w2_mode_spin, &w2_mode_scal,
 };
 
-/** switch the current mode */
-void w2_modes_switch(w2_e_mode new_mode);
+/** switch current mode (allow switching back to previous mode) */
+void w2_modes_call(w2_e_mode mode);
+/** switch current mode (replace current mode keeping history index) */
+void w2_modes_swap(w2_e_mode mode);
diff --git a/robot/sercomm.c b/robot/sercomm.c
index e972f28..c9c6194 100644
--- a/robot/sercomm.c
+++ b/robot/sercomm.c
@@ -71,7 +71,7 @@ void w2_cmd_mode_rx(w2_s_bin *data) {
 	w2_s_cmd_mode_rx *message = malloc(w2_cmd_sizeof(data->data, data->bytes));
 	memcpy(message, data->data, data->bytes);
 
-	w2_modes_switch(message->mode);
+	w2_modes_swap(message->mode);
 }
 
 void w2_cmd_sped_rx(w2_s_bin *data) { return; }
diff --git a/robot/setup.c b/robot/setup.c
index 1ecda43..06c8fa4 100644
--- a/robot/setup.c
+++ b/robot/setup.c
@@ -31,6 +31,9 @@ void w2_setup_main() {
 	// reset timer
 	time_reset();
 
+	// set default mode
+	w2_modes_swap(W2_M_HALT);
+
 	// indicate startup done
 	play("L50 c>c");
 }
diff --git a/shared/consts.h b/shared/consts.h
index f9d4c11..25ca94f 100644
--- a/shared/consts.h
+++ b/shared/consts.h
@@ -17,3 +17,5 @@
 #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)
diff --git a/shared/errors.h b/shared/errors.h
index 44b29a0..ac8e95f 100644
--- a/shared/errors.h
+++ b/shared/errors.h
@@ -47,6 +47,8 @@ typedef enum {
 	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 */
+	W2_E_WARN_MODE_HISTORY_BUFFER_IOB = 0x0a | W2_E_TYPE_WARN,
 } w2_e_errorcode;
 
 /**
diff --git a/shared/util.h b/shared/util.h
index 95f4c68..9e4d8ac 100644
--- a/shared/util.h
+++ b/shared/util.h
@@ -1,3 +1,7 @@
 #pragma once
 
+#define W2_MIN(a, b) (((a) < (b)) ? (a) : (b))
+#define W2_MAX(a, b) (((a) > (b)) ? (a) : (b))
+#define W2_RANGE(min, val, max) W2_MIN(max, W2_MAX(val, min))
+
 unsigned long w2_util_exp_mov_avg(unsigned long current_avg, unsigned long new_meas);