implement ping command

2 files changed, 19 insertions, 3 deletions

diff --git a/protocol.md b/protocol.md
index d41b00f..4db4032 100644
--- a/protocol.md
+++ b/protocol.md
@@ -27,12 +27,12 @@ and the starting byte don't count towards message length.
 opcodes are picked sequentially, but the direction bit (LSB) is reserved to
 indicate a transfer from robot to client (`tx`). this means that the opcode for
 a sensor data request would be `0x12`, but the response opcode would be `0x13`.
-these opcodes are stored as enum constants inside consts.h for code
+these opcodes are stored as enum constants inside shared/protocol.h for code
 readability.
 
 |code|name|implemented|directions|full name|
 |--:|---|:-:|:-:|---|
-|`0x00`|[PING](#ping)|no|`r <=> c`|<u>ping</u>
+|`0x00`|[PING](#ping)|yes|`r <=> c`|<u>ping</u>
 |`0x02`|[EXPT](#expt)|no|`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>
diff --git a/robot/sercomm.c b/robot/sercomm.c
index da632b5..44bf5f9 100644
--- a/robot/sercomm.c
+++ b/robot/sercomm.c
@@ -47,7 +47,23 @@ void w2_sercomm_append_msg(w2_s_bin *data) {
 	g_w2_sercomm_index = next_index;
 }
 
-void w2_scmd_ping_rx(w2_s_bin *data) { return; }
+void w2_scmd_ping_rx(w2_s_bin *data) {
+	w2_s_cmd_ping_rx *message = malloc(w2_scmd_length(data->data, data->bytes));
+	memcpy(message, data->data, data->bytes);
+
+	size_t return_size				 = sizeof(w2_s_cmd_ping_tx);
+	w2_s_cmd_ping_tx *return_message = malloc(return_size);
+	return_message->opcode			 = (message->opcode & W2_CMD_DIRECTION_MASK) | W2_CMDDIR_TX;
+	return_message->id				 = message->id;
+
+	w2_s_bin *return_message_bin = w2_bin_s_alloc(return_size, (uint8_t *)return_message);
+
+	w2_sercomm_append_msg(return_message_bin);
+
+	free(message);
+	free(return_message);
+	free(return_message_bin);
+}
 
 void w2_scmd_mode_rx(w2_s_bin *data) { return; }