From 266fd5ef0c45315fc0bf281a15630a9b4765e68c Mon Sep 17 00:00:00 2001 From: lonkaars Date: Thu, 26 May 2022 19:17:33 +0200 Subject: implement SENS command --- protocol.md | 8 ++++---- robot/modes.c | 5 ++--- robot/readme.md | 3 +++ robot/sercomm.c | 26 +++++++++++++++++++++++++- robot/sim.h | 2 +- robot/tests/sens.bin | 1 + shared/bin.c | 5 +++++ shared/bin.h | 9 +++++++++ shared/protocol.c | 2 -- shared/protocol.h | 7 +++++-- shared/serial_parse.c | 1 - 11 files changed, 55 insertions(+), 14 deletions(-) create mode 100644 robot/tests/sens.bin diff --git a/protocol.md b/protocol.md index c6cb3fb..7c7c843 100644 --- a/protocol.md +++ b/protocol.md @@ -41,7 +41,7 @@ readability. |`0x0c`|[BOMD](#bomd)|no|`r <=> c`|backorder modify |`0x0e`|[SRES](#sres)|no|`r <-- c`|soft reset |`0x10`|[MCFG](#mcfg)|no|`r <-- c`|map config -|`0x12`|[SENS](#sens)|no|`r <-> c`|sensor data +|`0x12`|[SENS](#sens)|yes|`r <-> c`|sensor data |`0x14`|[INFO](#info)|yes|`r <-> c`|info |`0x16`|[DISP](#disp)|no|`r <-- c`|display control |`0x18`|[PLAY](#play)|no|`r <-- c`|play midi @@ -277,15 +277,15 @@ later reference: requests sensor data -#### sensor data response (`r --> c`) (??? bytes) +#### sensor data response (`r --> c`) (44 bytes) |type|description| |-:|-| |`uint8_t`|opcode (`0x12 + 1`)| -|`???`|???| +|`w2_s_io_all`|sensor data (see shared/protocol.h for definition)| sensor data response. contains all sensor data (distance, ir, buttons) in one -packet. format yet to be determined. +packet. ### INFO diff --git a/robot/modes.c b/robot/modes.c index 8d3a099..9877f6e 100644 --- a/robot/modes.c +++ b/robot/modes.c @@ -1,8 +1,7 @@ -#include - +#include "modes.h" +#include "../shared/protocol.h" #include "../shared/util.h" #include "errcatch.h" -#include "modes.h" #include "sercomm.h" /** function pointer to current mode */ diff --git a/robot/readme.md b/robot/readme.md index c1cae72..b27c06f 100644 --- a/robot/readme.md +++ b/robot/readme.md @@ -99,6 +99,9 @@ this list will probably get updated from time to time: - general constants should be placed in `consts.h` - don't import `` directly, instead use `"orangutan_shim.h"` to keep code compatible with the simulator +- don't use ``, instead use `"../shared/protocol.h"`. this makes + sure that `bool` values are equal to `uint8_t`. they're functionally + identical. ## todo diff --git a/robot/sercomm.c b/robot/sercomm.c index 2786b85..2736030 100644 --- a/robot/sercomm.c +++ b/robot/sercomm.c @@ -4,6 +4,7 @@ #include "../shared/bin.h" #include "../shared/serial_parse.h" #include "hypervisor.h" +#include "io.h" #include "mode_dirc.h" #include "modes.h" #include "orangutan_shim.h" @@ -93,7 +94,30 @@ void w2_cmd_sres_rx(w2_s_bin *data) { return; } void w2_cmd_mcfg_rx(w2_s_bin *data) { return; } -void w2_cmd_sens_rx(w2_s_bin *data) { return; } +void w2_cmd_sens_rx(w2_s_bin *data) { + w2_s_cmd_sens_rx *message = malloc(w2_cmd_sizeof(data->data, data->bytes)); + memcpy(message, data->data, data->bytes); + + size_t return_size = sizeof(w2_s_cmd_sens_tx); + w2_s_cmd_sens_tx *return_message = malloc(return_size); + return_message->opcode = W2_CMD_SENS | W2_CMDDIR_TX; + memcpy((uint8_t *)&return_message->io, (uint8_t *)&g_w2_io, sizeof(w2_s_io_all)); + + for (int i = 0; i < 5; i++) w2_bin_repl_hton16(&return_message->io.qtr[i].range); + w2_bin_repl_hton16(&return_message->io.front_distance.detection); + w2_bin_repl_hton16(&return_message->io.side_distance.detection); + w2_bin_repl_hton16(&return_message->io.battery.charge_level); + w2_bin_repl_hton16((uint16_t *)&return_message->io.motor_left.speed); + w2_bin_repl_hton16((uint16_t *)&return_message->io.motor_right.speed); + + 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_cmd_info_rx(w2_s_bin *data) { w2_s_cmd_info_rx *message = malloc(w2_cmd_sizeof(data->data, data->bytes)); diff --git a/robot/sim.h b/robot/sim.h index f087517..ab1cd77 100644 --- a/robot/sim.h +++ b/robot/sim.h @@ -3,9 +3,9 @@ #include #include #include -#include #include "../shared/bin.h" +#include "../shared/protocol.h" extern bool g_w2_sim_headless; diff --git a/robot/tests/sens.bin b/robot/tests/sens.bin new file mode 100644 index 0000000..0f21683 --- /dev/null +++ b/robot/tests/sens.bin @@ -0,0 +1 @@ +ÿ \ No newline at end of file diff --git a/shared/bin.c b/shared/bin.c index 4b3dcc6..a0937e8 100644 --- a/shared/bin.c +++ b/shared/bin.c @@ -78,3 +78,8 @@ w2_s_bin *w2_bin_s_cat(w2_s_bin *a, w2_s_bin *b) { free(b); return c; } + +void w2_bin_repl_hton32(uint32_t *h32) { *h32 = w2_bin_hton32(*h32); } +void w2_bin_repl_hton16(uint16_t *h16) { *h16 = w2_bin_hton16(*h16); } +void w2_bin_repl_ntoh32(uint32_t *h32) { *h32 = w2_bin_ntoh32(*h32); } +void w2_bin_repl_ntoh16(uint16_t *h16) { *h16 = w2_bin_ntoh16(*h16); } diff --git a/shared/bin.h b/shared/bin.h index 2b65c95..af3a249 100644 --- a/shared/bin.h +++ b/shared/bin.h @@ -32,3 +32,12 @@ uint16_t w2_bin_hton16(uint16_t h16); uint32_t w2_bin_ntoh32(uint32_t n32); /** convert 16-bit value from network (big-endian) to host endian */ uint16_t w2_bin_ntoh16(uint16_t n16); + +/** replace 32-bit value from host endian to network (big-endian) */ +void w2_bin_repl_hton32(uint32_t *h32); +/** replace 16-bit value from host endian to network (big-endian) */ +void w2_bin_repl_hton16(uint16_t *h16); +/** replace 32-bit value from network (big-endian) to host endian */ +void w2_bin_repl_ntoh32(uint32_t *n32); +/** replace 16-bit value from network (big-endian) to host endian */ +void w2_bin_repl_ntoh16(uint16_t *n16); diff --git a/shared/protocol.c b/shared/protocol.c index 77ec466..fcc9fa4 100644 --- a/shared/protocol.c +++ b/shared/protocol.c @@ -1,5 +1,3 @@ -#include - #include "protocol.h" #ifdef W2_SIM #include "../robot/orangutan_shim.h" diff --git a/shared/protocol.h b/shared/protocol.h index b997ec6..7aa5e9f 100644 --- a/shared/protocol.h +++ b/shared/protocol.h @@ -1,12 +1,15 @@ #pragma once -#include #include #include #include "bin.h" #include "consts.h" +typedef uint8_t bool; +#define false 0 /* NOLINT */ +#define true 1 /* NOLINT */ + #define W2_SERIAL_START_BYTE 0xff #define W2_CMDDIR_RX 0 @@ -71,7 +74,7 @@ typedef struct { /** motor output struct */ typedef struct { - int speed; + int16_t speed; } w2_s_o_motor; /** underside led output struct */ diff --git a/shared/serial_parse.c b/shared/serial_parse.c index 89c5809..3bc8e3b 100644 --- a/shared/serial_parse.c +++ b/shared/serial_parse.c @@ -1,4 +1,3 @@ -#include #include #include "consts.h" -- cgit v1.2.3