implement SENS command0.2.0

11 files changed, 55 insertions, 14 deletions

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`|<u>b</u>ack<u>o</u>rder <u>m</u>o<u>d</u>ify
 |`0x0e`|[SRES](#sres)|no|`r <-- c`|<u>s</u>oft <u>res</u>et
 |`0x10`|[MCFG](#mcfg)|no|`r <-- c`|<u>m</u>ap <u>c</u>on<u>f</u>i<u>g</u>
-|`0x12`|[SENS](#sens)|no|`r <-> c`|<u>sens</u>or data
+|`0x12`|[SENS](#sens)|yes|`r <-> c`|<u>sens</u>or data
 |`0x14`|[INFO](#info)|yes|`r <-> c`|<u>info</u>
 |`0x16`|[DISP](#disp)|no|`r <-- c`|<u>disp</u>lay control
 |`0x18`|[PLAY](#play)|no|`r <-- c`|<u>play</u> 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 <stdbool.h>
-
+#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 `<pololu/orangutan.h>` directly, instead use
   `"orangutan_shim.h"` to keep code compatible with the simulator
+- don't use `<stdbool.h>`, 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 <stdio.h>
 #include <stdlib.h>
 #include <unistd.h>
-#include <stdbool.h>
 
 #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 <stdbool.h>
-
 #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 <stdbool.h>
 #include <stdint.h>
 #include <stdlib.h>
 
 #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 <stdbool.h>
 #include <string.h>
 
 #include "consts.h"