From 931cf5f2c3a0fdf947ba2c4b839bc886a43b4701 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Wed, 25 May 2022 19:16:41 +0200 Subject: implement ping command --- protocol.md | 4 ++-- robot/sercomm.c | 18 +++++++++++++++++- 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`|ping +|`0x00`|[PING](#ping)|yes|`r <=> c`|ping |`0x02`|[EXPT](#expt)|no|`r --> c`|exception |`0x04`|[MODE](#mode)|no|`r <=> c`|mode |`0x06`|[SPED](#sped)|no|`r <-- c`|speed 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; } -- cgit v1.2.3