1 files changed, 34 insertions, 9 deletions

diff --git a/robot/sercomm.c b/robot/sercomm.c
index 519568d..afde48a 100644
--- a/robot/sercomm.c
+++ b/robot/sercomm.c
@@ -20,6 +20,12 @@ 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", "");
@@ -31,6 +37,27 @@ void w2_sercomm_main() {
 		g_w2_serial_buffer_index = (g_w2_serial_buffer_index + 1) % W2_SERIAL_READ_BUFFER_SIZE;
 	}
 
+	// check time-out
+	if (!g_w2_ping_received && w2_hypervisor_time_end(W2_TIMER_PING) > W2_PING_TIMEOUT) {
+		g_w2_ping_timeout = true;
+		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) {
+		g_w2_ping_timeout = false;
+		g_w2_ping_received = false;
+		g_w2_ping_id = (uint8_t) rand();
+
+		W2_CREATE_MSG_BIN(w2_s_cmd_ping_tx, msg, bin);
+		msg->opcode = W2_CMD_PING | W2_CMDDIR_TX;
+		msg->id		= g_w2_ping_id;
+
+		w2_sercomm_append_msg(bin);
+		free(bin);
+
+		w2_hypervisor_time_start(W2_TIMER_PING);
+	}
+
 	// send data
 	while (g_w2_sercomm_offset != g_w2_sercomm_index) {
 		w2_s_bin *data = g_w2_sercomm_buffer[g_w2_sercomm_offset];
@@ -62,15 +89,14 @@ void w2_sercomm_append_msg(w2_s_bin *data) {
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Warray-bounds"
 
-void w2_cmd_ping_rx(w2_s_bin *data) {
-	W2_CAST_BIN(w2_s_cmd_ping_rx, data, req);
-
-	W2_CREATE_MSG_BIN(w2_s_cmd_ping_tx, res_msg, res_bin);
-	res_msg->opcode = W2_CMD_PING | W2_CMDDIR_TX;
-	res_msg->id		= req->id;
+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;
+}
 
-	w2_sercomm_append_msg(res_bin);
-	free(res_bin);
+void w2_cmd_ping_rx(w2_s_bin *data) {
+	w2_sercomm_append_msg(data);
 }
 
 void w2_cmd_mode_rx(w2_s_bin *data) {
@@ -150,7 +176,6 @@ void w2_cmd_cled_rx(w2_s_bin *data) { return; }
 
 #pragma GCC diagnostic pop
 
-void w2_cmd_ping_tx(w2_s_bin *data) {}
 void w2_cmd_expt_tx(w2_s_bin *data) {}
 void w2_cmd_mode_tx(w2_s_bin *data) {}
 void w2_cmd_cord_tx(w2_s_bin *data) {}