[WIP] stop on connection lost in dirc

11 files changed, 55 insertions, 39 deletions

diff --git a/client/main.c b/client/main.c
index 00f686a..b5af0e8 100644
--- a/client/main.c
+++ b/client/main.c
@@ -1,15 +1,13 @@
 #include "main.h"
-#include "../shared/errcatch.h"
 #include "../shared/consts.h"
+#include "../shared/errcatch.h"
+#include "commands.h"
 #include "serial.h"
 #include "setup.h"
-#include "ui.h"
 #include "time.h"
-#include "commands.h"
+#include "ui.h"
 
-w2_s_client_state g_w2_state = {
-	.ping_received = true
-};
+w2_s_client_state g_w2_state = {.ping_received = true};
 
 int main(int argc, char **argv) {
 	w2_client_setup(argc, argv);
@@ -21,12 +19,13 @@ int main(int argc, char **argv) {
 
 		if (!g_w2_state.ping_received && w2_timer_end(W2_TIMER_PING) > W2_PING_TIMEOUT) {
 			g_w2_state.ping_timeout = true;
-			g_w2_state.connected = false;
+			g_w2_state.connected	= false;
 			w2_errcatch_throw(W2_E_WARN_PING_TIMEOUT);
 		}
-		
-		if ((g_w2_state.ping_received && w2_timer_end(W2_TIMER_PING) > W2_PING_FREQUENCY) || g_w2_state.ping_timeout) {
-			g_w2_state.ping_timeout = false;
+
+		if ((g_w2_state.ping_received && w2_timer_end(W2_TIMER_PING) > W2_PING_FREQUENCY) ||
+			g_w2_state.ping_timeout) {
+			g_w2_state.ping_timeout	 = false;
 			g_w2_state.ping_received = false;
 			w2_send_ping();
 		}
diff --git a/client/serial.c b/client/serial.c
index 743fe76..112fd81 100644
--- a/client/serial.c
+++ b/client/serial.c
@@ -2,10 +2,10 @@
 
 #include "../shared/protocol.h"
 #include "../shared/serial_parse.h"
+#include "commands.h"
 #include "main.h"
 #include "serial.h"
 #include "time.h"
-#include "commands.h"
 
 void w2_serial_main() {
 	int temp;
@@ -20,13 +20,11 @@ void w2_cmd_ping_rx(w2_s_bin *data) {
 
 	g_w2_state.ping			 = w2_timer_end(W2_TIMER_PING);
 	g_w2_state.ping_received = true;
-	g_w2_state.ping_timeout = false;
-	g_w2_state.connected = true;
+	g_w2_state.ping_timeout	 = false;
+	g_w2_state.connected	 = true;
 }
 
-void w2_cmd_ping_tx(w2_s_bin *data) {
-	w2_send_bin(data);
-}
+void w2_cmd_ping_tx(w2_s_bin *data) { w2_send_bin(data); }
 
 void w2_cmd_expt_tx(w2_s_bin *data) {}
 void w2_cmd_mode_tx(w2_s_bin *data) {
diff --git a/client/ui_dirc.c b/client/ui_dirc.c
index 675913a..ed69cd2 100644
--- a/client/ui_dirc.c
+++ b/client/ui_dirc.c
@@ -81,7 +81,9 @@ void w2_ui_dirc_paint(int left, int right) {
 			 "\n"
 			 " <q>      <w>       <e>   forward\n"
 			 " <a>      <s>       <d>   backward\n"
-			 "left     both      right\n");
+			 "left     both      right\n"
+			 "\n"
+			 "<space> send dirc mode command");
 }
 
 void w2_ui_dirc(bool first) {
@@ -96,6 +98,7 @@ void w2_ui_dirc(bool first) {
 		if (ch == 'd' || ch == 's') lb++;
 		if (ch == 'q' || ch == 'w') rf++;
 		if (ch == 'a' || ch == 's') rb++;
+		if (ch == ' ') w2_send_mode(W2_M_DIRC);
 	}
 
 	int drive_l = w2_dirc_motor_l(lf, lb);
@@ -104,6 +107,6 @@ void w2_ui_dirc(bool first) {
 	drive_l += drive_r * W2_DIRC_STP;
 	drive_r += drive_l * W2_DIRC_STP;
 
-	// w2_send_dirc(drive_l, drive_r);
+	w2_send_dirc(drive_l, drive_r);
 	w2_ui_dirc_paint(drive_l, drive_r);
 }
diff --git a/robot/errcatch.c b/robot/errcatch.c
index 0f809f9..77fcfd3 100644
--- a/robot/errcatch.c
+++ b/robot/errcatch.c
@@ -19,21 +19,21 @@ void w2_errcatch_handle_error(w2_s_error *error) {
 		default: {
 			g_w2_error_uncaught = true;
 #ifdef W2_SIM
-			simwarn("Uncaught/unhandled error found with code 0x%02x\n", error->code);
+			simwarn("Uncaught/unhandled error found with code 0x%02x", error->code);
+			if (error->message_length > 0) fprintf(stderr, " and message \"%.*s\"", error->message_length, error->message);
+			fprintf(stderr, "\n");
 #endif
 		}
 	}
 
 	// 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);
+	W2_CREATE_MSG_SIZE_BIN(w2_s_cmd_expt_tx, sizeof(w2_s_cmd_expt_tx) + sizeof(uint8_t) * error->message_length, msg, msg_bin);
 	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/hypervisor.c b/robot/hypervisor.c
index 478d3a5..efdceec 100644
--- a/robot/hypervisor.c
+++ b/robot/hypervisor.c
@@ -14,6 +14,12 @@ 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};
 
+unsigned int g_w2_ping_ms = 0;
+uint8_t g_w2_ping_id = 0;
+bool g_w2_ping_received = true;
+bool g_w2_ping_timeout = false;
+bool g_w2_connected = false;
+
 void w2_hypervisor_main() {
 #ifdef W2_SIM
 	w2_sim_cycle_begin();
diff --git a/robot/mode_dirc.c b/robot/mode_dirc.c
index 5988816..397d44d 100644
--- a/robot/mode_dirc.c
+++ b/robot/mode_dirc.c
@@ -1,13 +1,18 @@
 #include "mode_dirc.h"
 #include "io.h"
-
+#include "hypervisor.h"
+#include "modes.h"
+#include "../shared/util.h"
 #include "orangutan_shim.h"
 
 int16_t g_w2_mode_dirc_motor_l = 0;
 int16_t g_w2_mode_dirc_motor_r = 0;
+double g_w2_mode_dirc_power = 1.0;
 
 void w2_mode_dirc() {
-	set_motors(g_w2_mode_dirc_motor_l, g_w2_mode_dirc_motor_r);
-	g_w2_io.motor_left.speed  = g_w2_mode_dirc_motor_l;
-	g_w2_io.motor_right.speed = g_w2_mode_dirc_motor_r;
+	// if (g_w2_connected) g_w2_mode_dirc_power = 1.0;
+	// else g_w2_mode_dirc_power = W2_MAX(0, g_w2_mode_dirc_power - 0.01);
+
+	// if (g_w2_mode_dirc_power == 0.0) w2_modes_call(W2_M_HALT);
+	set_motors(g_w2_mode_dirc_motor_l * g_w2_mode_dirc_power, g_w2_mode_dirc_motor_r * g_w2_mode_dirc_power);
 }
diff --git a/robot/mode_dirc.h b/robot/mode_dirc.h
index 12c967a..bf02270 100644
--- a/robot/mode_dirc.h
+++ b/robot/mode_dirc.h
@@ -6,6 +6,7 @@
 
 extern int16_t g_w2_mode_dirc_motor_l;
 extern int16_t g_w2_mode_dirc_motor_r;
+extern double g_w2_mode_dirc_power;
 
 /**
  * direct control mode
diff --git a/robot/sercomm.c b/robot/sercomm.c
index afde48a..6e191d2 100644
--- a/robot/sercomm.c
+++ b/robot/sercomm.c
@@ -20,12 +20,6 @@ char g_w2_serial_buffer[W2_SERIAL_READ_BUFFER_SIZE] = {0};
 uint8_t g_w2_serial_buffer_index					= 0;
 uint8_t g_w2_serial_buffer_head						= 0;
 
-unsigned int g_w2_ping_ms = 0;
-uint8_t g_w2_ping_id = 0;
-bool g_w2_ping_received = true;
-bool g_w2_ping_timeout = false;
-bool g_w2_connected = false;
-
 void w2_sercomm_main() {
 #ifdef W2_SIM
 	simprintfunc("w2_sercomm_main", "");
@@ -40,10 +34,11 @@ void w2_sercomm_main() {
 	// check time-out
 	if (!g_w2_ping_received && w2_hypervisor_time_end(W2_TIMER_PING) > W2_PING_TIMEOUT) {
 		g_w2_ping_timeout = true;
+		g_w2_connected = false;
 		w2_errcatch_throw(W2_E_WARN_PING_TIMEOUT);
 	}
 	// send ping every W2_TIMER_PING ms
-	if ((g_w2_ping_received && w2_hypervisor_time_end(W2_TIMER_PING) > 1000) || g_w2_ping_timeout) {
+	if ((g_w2_ping_received && w2_hypervisor_time_end(W2_TIMER_PING) > W2_PING_FREQUENCY) || g_w2_ping_timeout) {
 		g_w2_ping_timeout = false;
 		g_w2_ping_received = false;
 		g_w2_ping_id = (uint8_t) rand();
@@ -62,7 +57,7 @@ void w2_sercomm_main() {
 	while (g_w2_sercomm_offset != g_w2_sercomm_index) {
 		w2_s_bin *data = g_w2_sercomm_buffer[g_w2_sercomm_offset];
 #ifdef W2_SIM
-		w2_sim_print_serial(data);
+		if (DBG_ENABLE_SERIAL) w2_sim_print_serial(data);
 #endif
 		serial_send_blocking("\xff", 1);
 		for (uint8_t i = 0; i < data->bytes; i++) {
@@ -89,10 +84,19 @@ void w2_sercomm_append_msg(w2_s_bin *data) {
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Warray-bounds"
 
+#include <stdlib.h>
+#include <string.h>
+
+
 void w2_cmd_ping_tx(w2_s_bin *data) {
 	g_w2_ping_ms = w2_hypervisor_time_end(W2_TIMER_PING);
 	g_w2_ping_received = true;
 	g_w2_ping_timeout = false;
+	g_w2_connected = true;
+
+	char buf[32];
+	sprintf(buf, "rec: %i, tim: %i, con: %i", g_w2_ping_received, g_w2_ping_timeout, g_w2_connected);
+	w2_errcatch_throw_msg(0x69, strlen(buf), buf);
 }
 
 void w2_cmd_ping_rx(w2_s_bin *data) {
diff --git a/robot/sim.h b/robot/sim.h
index 14f0f74..7d7c091 100644
--- a/robot/sim.h
+++ b/robot/sim.h
@@ -14,9 +14,9 @@
 #define DBG_ENABLE_SIMWARN (1)
 #define DBG_ENABLE_SIMINFO (1)
 #define DBG_ENABLE_CYCLEINFO (0)
-#define DBG_ENABLE_SERIAL (1)
+#define DBG_ENABLE_SERIAL (0)
 
-#define DBG_CYCLE_DELAY (1e3)
+#define DBG_CYCLE_DELAY (10e3)
 #define DBG_MAX_CYCLES (-1)
 
 // debug print options
diff --git a/shared/consts.h b/shared/consts.h
index 3f792b4..cd6dff1 100644
--- a/shared/consts.h
+++ b/shared/consts.h
@@ -22,7 +22,7 @@
 /** size of the serial communication buffer (in messages, not bytes) */
 #define W2_SERCOMM_BUFFER_SIZE (16)
 /** size of mode history buffer */
-#define W2_MODE_HISTORY_BUFFER_SIZE (4)
+#define W2_MODE_HISTORY_BUFFER_SIZE (8)
 
 /** max logic module execution time in milliseconds */
 #define W2_MAX_MODULE_CYCLE_MS (20)
diff --git a/shared/serial_parse.c b/shared/serial_parse.c
index 54b9ab5..d07c67f 100644
--- a/shared/serial_parse.c
+++ b/shared/serial_parse.c
@@ -51,7 +51,7 @@ void w2_cmd_handler(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t data_lengt
 		w2_errcatch_throw(W2_E_WARN_SERIAL_NOISY);
 	} else {
 #ifdef W2_SIM
-		w2_sim_print_serial(copy);
+		if (DBG_ENABLE_SERIAL) w2_sim_print_serial(copy);
 #endif
 		handler(copy);
 	}