aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--robot/sercomm.c61
-rw-r--r--robot/sercomm.h23
-rw-r--r--robot/setup.c2
-rw-r--r--robot/sim.c31
-rw-r--r--robot/sim.h9
-rw-r--r--shared/bin.c12
-rw-r--r--shared/bin.h25
-rw-r--r--shared/protocol.c92
-rw-r--r--shared/protocol.h213
-rw-r--r--shared/serial_parse.c17
10 files changed, 363 insertions, 122 deletions
diff --git a/robot/sercomm.c b/robot/sercomm.c
index f4b7eb5..a1aa31c 100644
--- a/robot/sercomm.c
+++ b/robot/sercomm.c
@@ -46,3 +46,64 @@ void w2_sercomm_append_msg(w2_s_bin *data) {
if (g_w2_sercomm_buffer_full) return;
g_w2_sercomm_index = next_index;
}
+
+void w2_scmd_ping_rx(w2_s_bin *data) {
+ return;
+}
+
+void w2_scmd_mode_rx(w2_s_bin *data) {
+ return;
+}
+
+void w2_scmd_sped_rx(w2_s_bin *data) {
+ return;
+}
+
+void w2_scmd_dirc_rx(w2_s_bin *data) {
+ return;
+}
+
+void w2_scmd_cord_rx(w2_s_bin *data) {
+ return;
+}
+
+void w2_scmd_bomd_rx(w2_s_bin *data) {
+ return;
+}
+
+void w2_scmd_sres_rx(w2_s_bin *data) {
+ return;
+}
+
+void w2_scmd_mcfg_rx(w2_s_bin *data) {
+ return;
+}
+
+void w2_scmd_sens_rx(w2_s_bin *data) {
+ return;
+}
+
+void w2_scmd_info_rx(w2_s_bin *data) {
+ return;
+}
+
+void w2_scmd_disp_rx(w2_s_bin *data) {
+ return;
+}
+
+void w2_scmd_play_rx(w2_s_bin *data) {
+ return;
+}
+
+void w2_scmd_cled_rx(w2_s_bin *data) {
+ return;
+}
+
+void w2_scmd_ping_tx(w2_s_bin *data) { }
+void w2_scmd_expt_tx(w2_s_bin *data) { }
+void w2_scmd_mode_tx(w2_s_bin *data) { }
+void w2_scmd_cord_tx(w2_s_bin *data) { }
+void w2_scmd_bomd_tx(w2_s_bin *data) { }
+void w2_scmd_sens_tx(w2_s_bin *data) { }
+void w2_scmd_info_tx(w2_s_bin *data) { }
+
diff --git a/robot/sercomm.h b/robot/sercomm.h
index a53d3e2..d717f01 100644
--- a/robot/sercomm.h
+++ b/robot/sercomm.h
@@ -26,27 +26,6 @@ extern uint8_t g_w2_serial_buffer_head;
*/
void w2_sercomm_main();
+/** append binary message to send buffer */
void w2_sercomm_append_msg(w2_s_bin *data);
-w2_s_bin w2_sercomm_struct_pack(w2_s_property_list generic_struct);
-
-w2_s_bin w2_sercomm_rx_ping();
-w2_s_bin w2_sercomm_tx_ping();
-w2_s_bin w2_sercomm_tx_expt();
-w2_s_bin w2_sercomm_rx_mode();
-w2_s_bin w2_sercomm_tx_mode();
-w2_s_bin w2_sercomm_rx_sped();
-w2_s_bin w2_sercomm_rx_dirc();
-w2_s_bin w2_sercomm_rx_cord();
-w2_s_bin w2_sercomm_tx_cord();
-w2_s_bin w2_sercomm_rx_bomd();
-w2_s_bin w2_sercomm_tx_bomd();
-w2_s_bin w2_sercomm_rx_sres();
-w2_s_bin w2_sercomm_rx_mcfg();
-w2_s_bin w2_sercomm_rx_sens();
-w2_s_bin w2_sercomm_tx_sens();
-w2_s_bin w2_sercomm_rx_info();
-w2_s_bin w2_sercomm_tx_info();
-w2_s_bin w2_sercomm_rx_disp();
-w2_s_bin w2_sercomm_rx_play();
-w2_s_bin w2_sercomm_rx_cled();
diff --git a/robot/setup.c b/robot/setup.c
index 6dfef50..944ea68 100644
--- a/robot/setup.c
+++ b/robot/setup.c
@@ -24,7 +24,7 @@ void w2_setup_main() {
// clear lcd
clear();
- // start serial input
+ // start serial i/o
serial_set_baud_rate(W2_SERIAL_BAUD);
serial_receive_ring(g_w2_serial_buffer, W2_SERIAL_READ_BUFFER_SIZE);
diff --git a/robot/sim.c b/robot/sim.c
index 8af672e..57e0ef5 100644
--- a/robot/sim.c
+++ b/robot/sim.c
@@ -7,11 +7,34 @@
#include "sim.h"
#include "../shared/consts.h"
+#include "../shared/protocol.h"
#include "sercomm.h"
struct timespec reference_time; // NOLINT
bool g_w2_sim_headless = false;
+static const char* const W2_CMD_NAMES[] = {
+ "PING",
+ "EXPT",
+ "MODE",
+ "SPED",
+ "DIRC",
+ "CORD",
+ "BOMD",
+ "SRES",
+ "MCFG",
+ "SENS",
+ "INFO",
+ "DISP",
+ "PLAY",
+ "CLED",
+};
+
+static const char* const W2_CMD_DIRECTIONS[] = {
+ "RX",
+ "TX",
+};
+
void time_reset() {
simprintfunc("time_reset", "");
clock_gettime(CLOCK_MONOTONIC, &reference_time);
@@ -107,3 +130,11 @@ void w2_sim_cycle_begin() {
return;
}
+void w2_sim_print_serial(w2_s_bin *data) {
+ if (g_w2_sim_headless) return;
+ simprintf(COL_GRN "[%s_%s]" COL_RST, W2_CMD_NAMES[data->data[0] >> 1], W2_CMD_DIRECTIONS[data->data[0] & W2_CMD_DIRECTION_MASK]);
+ for (int i = 0; i < data->bytes; i++)
+ printf(" %02x", data->data[i]);
+ printf("\n");
+}
+
diff --git a/robot/sim.h b/robot/sim.h
index 336592b..6e2498d 100644
--- a/robot/sim.h
+++ b/robot/sim.h
@@ -5,6 +5,8 @@
#include <unistd.h>
#include <stdbool.h>
+#include "../shared/bin.h"
+
extern bool g_w2_sim_headless;
// debug fine-tuning
@@ -45,11 +47,12 @@ unsigned long get_ms(); // NOLINT
void red_led(unsigned char on); // NOLINT
void green_led(unsigned char on); // NOLINT
void clear(); // NOLINT
-void play(const char* melody); // NOLINT
+void play(const char *melody); // NOLINT
void serial_set_baud_rate(unsigned int rate); // NOLINT
-void serial_send(char* message, unsigned int length); // NOLINT
-void serial_receive_ring(char* buffer, unsigned char size); // NOLINT
+void serial_send(char *message, unsigned int length); // NOLINT
+void serial_receive_ring(char *buffer, unsigned char size); // NOLINT
unsigned char serial_get_received_bytes(); // NOLINT
void w2_sim_setup(int argc, char **argv);
void w2_sim_cycle_begin();
+void w2_sim_print_serial(w2_s_bin *data);
diff --git a/shared/bin.c b/shared/bin.c
index 44c2e94..dac8415 100644
--- a/shared/bin.c
+++ b/shared/bin.c
@@ -15,8 +15,6 @@
#define _BYTE_2 ((uint32_t)(0xff << (_SHIFT_2B)))
#define _BYTE_3 ((uint32_t)(0xff << (_SHIFT_3B)))
-const uint8_t W2_STRUCT_T_SIZES[] = {sizeof(uint8_t), sizeof(uint16_t), sizeof(uint32_t)};
-
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wshift-count-overflow"
w2_s_bin *w2_bin_from_uint8_t(uint8_t data) {
@@ -70,3 +68,13 @@ w2_s_bin *w2_bin_s_alloc(uint16_t bytes, uint8_t *data) {
memcpy(&temp->data, data, bytes);
return temp;
}
+
+w2_s_bin *w2_bin_s_cat(w2_s_bin *a, w2_s_bin *b) {
+ uint8_t data[a->bytes + b->bytes];
+ memcpy(data, a->data, a->bytes);
+ memcpy(data + a->bytes, b->data, b->bytes);
+ w2_s_bin* c = w2_bin_s_alloc(a->bytes + b->bytes, data);
+ free(a);
+ free(b);
+ return c;
+}
diff --git a/shared/bin.h b/shared/bin.h
index d9eacd7..2b65c95 100644
--- a/shared/bin.h
+++ b/shared/bin.h
@@ -10,18 +10,6 @@
extern uint8_t g_w2_endianness;
-#define W2_T_UINT8_T (0)
-#define W2_T_UINT16_T (1)
-#define W2_T_UINT32_T (2)
-
-enum w2_e_struct_types {
- W2_ST_UINT8_T,
- W2_ST_UINT16_T,
- W2_ST_UINT32_T,
-};
-
-extern const uint8_t W2_STRUCT_T_SIZES[];
-
typedef struct {
uint16_t bytes;
uint8_t data[];
@@ -29,17 +17,8 @@ typedef struct {
/** allocate new w2_s_bin struct and fill with `*data` for `bytes` bytes */
w2_s_bin *w2_bin_s_alloc(uint16_t bytes, uint8_t *data);
-
-typedef struct {
- enum w2_e_struct_types type;
- uint16_t length;
- const uint8_t *data;
-} w2_s_struct_property;
-
-typedef struct {
- uint16_t length;
- w2_s_struct_property *properties[];
-} w2_s_property_list;
+/** concatenate 2 w2_s_bin structs, deallocates `a` and `b` */
+w2_s_bin *w2_bin_s_cat(w2_s_bin *a, w2_s_bin *b);
w2_s_bin *w2_bin_from_uint8_t(uint8_t data);
w2_s_bin *w2_bin_from_uint16_t(uint16_t data);
diff --git a/shared/protocol.c b/shared/protocol.c
index c634a09..af38804 100644
--- a/shared/protocol.c
+++ b/shared/protocol.c
@@ -1,9 +1,91 @@
+#include <stdbool.h>
+
#include "protocol.h"
+#ifdef W2_SIM
+#include "../robot/orangutan_shim.h"
+#endif
+
+uint8_t w2_scmd_length(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t data_length) {
+ if ((data[0] & W2_CMD_CODE_MASK) == W2_CMD_PING) return 2;
+
+ if (data[0] == (W2_CMD_MODE | W2_CMDDIR_RX)) return 2;
+ if (data[0] == (W2_CMD_MODE | W2_CMDDIR_TX)) return 2;
+
+ if (data[0] == (W2_CMD_SPED | W2_CMDDIR_RX)) return 2;
+
+ if (data[0] == (W2_CMD_DIRC | W2_CMDDIR_RX)) return 5;
+
+ if (data[0] == (W2_CMD_CORD | W2_CMDDIR_RX)) return 6;
+ if (data[0] == (W2_CMD_CORD | W2_CMDDIR_TX)) return 6;
+
+ if (data[0] == (W2_CMD_BOMD | W2_CMDDIR_RX)) return 9;
+ if (data[0] == (W2_CMD_BOMD | W2_CMDDIR_TX)) return 6;
+
+ if (data[0] == (W2_CMD_SRES | W2_CMDDIR_RX)) return 2;
+
+ if (data[0] == (W2_CMD_SENS | W2_CMDDIR_RX)) return 1;
+ if (data[0] == (W2_CMD_SENS | W2_CMDDIR_TX)) return 0;
+
+ if (data[0] == (W2_CMD_INFO | W2_CMDDIR_RX)) return 1;
+ if (data[0] == (W2_CMD_INFO | W2_CMDDIR_TX)) return 41;
+
+ w2_s_bin *copy = w2_bin_s_alloc(data_length, data);
+ uint8_t length = 1;
-W2_PROTOCOL_DEFINE(W2_PROTOCOL_CMD_PING_RX)
+ if (data[0] == (W2_CMD_EXPT | W2_CMDDIR_TX)) length = w2_scmd_expt_tx_length(copy);
+ if (data[0] == (W2_CMD_MCFG | W2_CMDDIR_RX)) length = w2_scmd_mcfg_rx_length(copy);
-w2_s_cmd_ping_rx *w2_protocol_parse_cmd_ping_rx(w2_s_bin *data) {
- w2_s_cmd_ping_rx *parsed = malloc(data->bytes);
- memcpy(parsed, data->data, data->bytes);
- return parsed;
+ free(copy);
+
+ return length;
+}
+
+uint8_t w2_scmd_expt_tx_length(w2_s_bin *data) {
+ return 3 + (data->bytes > 2 ? (sizeof(uint8_t) * data->data[2]) : 0);
}
+
+uint8_t w2_scmd_mcfg_rx_length(w2_s_bin *data) {
+ // TODO 3 = sizeof(feature) -> protocol.md
+ return 4 + (data->bytes > 3 ? (3 * data->data[3]) : 0);
+}
+
+void w2_scmd_handler(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t data_length) {
+ w2_s_bin *copy = w2_bin_s_alloc(data_length, data);
+ bool unknown = false;
+
+ switch(data[0]) {
+ case W2_CMD_PING | W2_CMDDIR_RX: { w2_scmd_ping_rx(copy); break; }
+ case W2_CMD_PING | W2_CMDDIR_TX: { w2_scmd_ping_tx(copy); break; }
+ case W2_CMD_EXPT | W2_CMDDIR_TX: { w2_scmd_expt_tx(copy); break; }
+ case W2_CMD_MODE | W2_CMDDIR_RX: { w2_scmd_mode_rx(copy); break; }
+ case W2_CMD_MODE | W2_CMDDIR_TX: { w2_scmd_mode_tx(copy); break; }
+ case W2_CMD_SPED | W2_CMDDIR_RX: { w2_scmd_sped_rx(copy); break; }
+ case W2_CMD_DIRC | W2_CMDDIR_RX: { w2_scmd_dirc_rx(copy); break; }
+ case W2_CMD_CORD | W2_CMDDIR_RX: { w2_scmd_cord_rx(copy); break; }
+ case W2_CMD_CORD | W2_CMDDIR_TX: { w2_scmd_cord_tx(copy); break; }
+ case W2_CMD_BOMD | W2_CMDDIR_RX: { w2_scmd_bomd_rx(copy); break; }
+ case W2_CMD_BOMD | W2_CMDDIR_TX: { w2_scmd_bomd_tx(copy); break; }
+ case W2_CMD_SRES | W2_CMDDIR_RX: { w2_scmd_sres_rx(copy); break; }
+ case W2_CMD_MCFG | W2_CMDDIR_RX: { w2_scmd_mcfg_rx(copy); break; }
+ case W2_CMD_SENS | W2_CMDDIR_RX: { w2_scmd_sens_rx(copy); break; }
+ case W2_CMD_SENS | W2_CMDDIR_TX: { w2_scmd_sens_tx(copy); break; }
+ case W2_CMD_INFO | W2_CMDDIR_RX: { w2_scmd_info_rx(copy); break; }
+ case W2_CMD_INFO | W2_CMDDIR_TX: { w2_scmd_info_tx(copy); break; }
+ case W2_CMD_DISP | W2_CMDDIR_RX: { w2_scmd_disp_rx(copy); break; }
+ case W2_CMD_PLAY | W2_CMDDIR_RX: { w2_scmd_play_rx(copy); break; }
+ case W2_CMD_CLED | W2_CMDDIR_RX: { w2_scmd_cled_rx(copy); break; }
+ default: {
+#ifdef W2_SIM
+ simwarn("unknown serial message with code 0x%02x\n", data[0]);
+#endif
+ unknown = true;
+ }
+ }
+
+#ifdef W2_SIM
+ if (!unknown) w2_sim_print_serial(copy);
+#endif
+
+ free(copy);
+}
+
diff --git a/shared/protocol.h b/shared/protocol.h
index 775c2aa..a9967d7 100644
--- a/shared/protocol.h
+++ b/shared/protocol.h
@@ -5,6 +5,7 @@
#include <stdlib.h>
#include "bin.h"
+#include "consts.h"
#define W2_SERIAL_START_BYTE 0xff
@@ -14,7 +15,7 @@
#define W2_CMD_CODE_MASK (~1)
#define W2_CMD_DIRECTION_MASK (1)
-enum w2_e_serial_commands {
+enum w2_e_scmds {
/** ping command */
W2_CMD_PING = 0x00,
/** exception command */
@@ -45,59 +46,167 @@ enum w2_e_serial_commands {
W2_CMD_CLED = 0x1a,
};
-// TODO
-// array met indicies die structs opslaan met eigenschappen over de protocol bericht
-//
-// belangrijke eigenschappen:
-// lengte!!!
-// generic struct (voor parsen)
-// parse functie
-// dump functie
-//
-//
-// als het kan deze allemaal met macro's op deze manier definieren:
-//
-#define W2_PROTOCOL_CMD(name, direction, ...)
-#define W2_PROTOCOL_DEFINE(a)
-#define W2_PROTOCOL_DECLARE(a)
-#define W2_PROTOCOL_PROP(type, name)
-
-#define W2_CMDDIR_NAME_0 rx
-#define W2_CMDDIR_NAME_1 tx
-
-#define W2_CMDDIR(dir) W2_CMDDIR_NAME_##dir
-
-#define W2_PROTOCOL_UINT8_T
-#define W2_PROTOCOL_UINT16_T
-#define W2_PROTOCOL_UINT32_T
-#define W2_PROTOCOL_INT8_T
-#define W2_PROTOCOL_INT16_T
-#define W2_PROTOCOL_INT32_T
-
-#define W2_PROTOCOL_UINT8_T_TYPE uint8_t
-#define W2_PROTOCOL_UINT16_T_TYPE uint16_t
-#define W2_PROTOCOL_UINT32_T_TYPE uint32_t
-#define W2_PROTOCOL_INT8_T_TYPE int8_t
-#define W2_PROTOCOL_INT16_T_TYPE int16_t
-#define W2_PROTOCOL_INT32_T_TYPE int32_t
-
-#define W2_PROTOCOL_UINT8_T_SIZE 1
-#define W2_PROTOCOL_UINT16_T_SIZE 2
-#define W2_PROTOCOL_UINT32_T_SIZE 4
-#define W2_PROTOCOL_INT8_T_SIZE 1
-#define W2_PROTOCOL_INT16_T_SIZE 2
-#define W2_PROTOCOL_INT32_T_SIZE 4
-
-#define W2_PROTOCOL_CMD_PING_RX \
- W2_PROTOCOL_CMD(ping, W2_CMDDIR_RX, \
- W2_PROTOCOL_PROP(W2_PROTOCOL_UINT8_T, opcode) \
- W2_PROTOCOL_PROP(W2_PROTOCOL_UINT8_T, id))
-
-W2_PROTOCOL_DECLARE(W2_PROTOCOL_CMD_PING_RX)
-
typedef struct {
uint8_t opcode;
uint8_t id;
} w2_s_cmd_ping_rx;
-w2_s_cmd_ping_rx *w2_protocol_parse_cmd_ping_rx(w2_s_bin *data);
+typedef struct {
+ uint8_t opcode;
+ uint8_t id;
+} w2_s_cmd_ping_tx;
+
+typedef struct {
+ uint8_t opcode;
+ uint8_t error;
+ uint8_t length;
+ uint8_t message[];
+} w2_s_cmd_expt_tx;
+
+typedef struct {
+ uint8_t opcode;
+ uint8_t mode;
+} w2_s_cmd_mode_rx;
+
+typedef struct {
+ uint8_t opcode;
+ uint8_t mode;
+} w2_s_cmd_mode_tx;
+
+typedef struct {
+ uint8_t opcode;
+ uint8_t speed;
+} w2_s_cmd_sped_rx;
+
+typedef struct {
+ uint8_t opcode;
+ uint16_t left;
+ uint16_t right;
+} w2_s_cmd_dirc_rx;
+
+typedef struct {
+ uint8_t opcode;
+ uint32_t position;
+ uint8_t orientation;
+} w2_s_cmd_cord_rx;
+
+typedef struct {
+ uint8_t opcode;
+ uint32_t position;
+ uint8_t orientation;
+} w2_s_cmd_cord_tx;
+
+typedef struct {
+ uint8_t opcode;
+ uint32_t id;
+ uint32_t position;
+} w2_s_cmd_bomd_rx;
+
+typedef struct {
+ uint8_t opcode;
+ uint32_t id;
+ uint8_t status;
+} w2_s_cmd_bomd_tx;
+
+typedef struct {
+ uint8_t opcode;
+ uint8_t type;
+} w2_s_cmd_sres_rx;
+
+typedef struct {
+ uint16_t position;
+ uint8_t kind;
+} w2_s_cmd_mcfg_feature;
+
+typedef struct {
+ uint8_t opcode;
+ uint8_t width;
+ uint8_t height;
+ uint8_t length;
+ w2_s_cmd_mcfg_feature features[];
+} w2_s_cmd_mcfg_rx;
+
+typedef struct {
+ uint8_t opcode;
+} w2_s_cmd_sens_rx;
+
+typedef struct {
+ uint8_t opcode;
+ // TODO: sensor data
+} w2_s_cmd_sens_tx;
+
+typedef struct {
+ uint8_t opcode;
+} w2_s_cmd_info_rx;
+
+typedef struct {
+ uint8_t opcode;
+ uint8_t build_str[32];
+ uint8_t errcatch_ms;
+ uint8_t io_ms;
+ uint8_t sercomm_ms;
+ uint8_t mode_ms;
+ uint32_t uptime_s;
+} w2_s_cmd_info_tx;
+
+typedef struct {
+} w2_s_cmd_disp_rx;
+
+typedef struct {
+} w2_s_cmd_play_rx;
+
+typedef struct {
+} w2_s_cmd_cled_rx;
+
+
+/** global handler for complete messages */
+void w2_scmd_handler(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t length);
+/** calculate message length */
+uint8_t w2_scmd_length(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t length);
+
+/** handler for ping_rx (on complete message) */
+void w2_scmd_ping_rx(w2_s_bin *data);
+/** handler for ping_tx (on complete message) */
+void w2_scmd_ping_tx(w2_s_bin *data);
+/** handler for expt_tx (on complete message) */
+void w2_scmd_expt_tx(w2_s_bin *data);
+/** handler for mode_rx (on complete message) */
+void w2_scmd_mode_rx(w2_s_bin *data);
+/** handler for mode_tx (on complete message) */
+void w2_scmd_mode_tx(w2_s_bin *data);
+/** handler for sped_rx (on complete message) */
+void w2_scmd_sped_rx(w2_s_bin *data);
+/** handler for dirc_rx (on complete message) */
+void w2_scmd_dirc_rx(w2_s_bin *data);
+/** handler for cord_rx (on complete message) */
+void w2_scmd_cord_rx(w2_s_bin *data);
+/** handler for cord_tx (on complete message) */
+void w2_scmd_cord_tx(w2_s_bin *data);
+/** handler for bomd_rx (on complete message) */
+void w2_scmd_bomd_rx(w2_s_bin *data);
+/** handler for bomd_tx (on complete message) */
+void w2_scmd_bomd_tx(w2_s_bin *data);
+/** handler for sres_rx (on complete message) */
+void w2_scmd_sres_rx(w2_s_bin *data);
+/** handler for mcfg_rx (on complete message) */
+void w2_scmd_mcfg_rx(w2_s_bin *data);
+/** handler for sens_rx (on complete message) */
+void w2_scmd_sens_rx(w2_s_bin *data);
+/** handler for sens_tx (on complete message) */
+void w2_scmd_sens_tx(w2_s_bin *data);
+/** handler for info_rx (on complete message) */
+void w2_scmd_info_rx(w2_s_bin *data);
+/** handler for info_tx (on complete message) */
+void w2_scmd_info_tx(w2_s_bin *data);
+/** handler for disp_rx (on complete message) */
+void w2_scmd_disp_rx(w2_s_bin *data);
+/** handler for play_rx (on complete message) */
+void w2_scmd_play_rx(w2_s_bin *data);
+/** handler for cled_rx (on complete message) */
+void w2_scmd_cled_rx(w2_s_bin *data);
+
+/** calculate message length for expt_tx (incomplete message) */
+uint8_t w2_scmd_expt_tx_length(w2_s_bin *data);
+/** calculate message length for mcfg_rx (incomplete message) */
+uint8_t w2_scmd_mcfg_rx_length(w2_s_bin *data);
+
diff --git a/shared/serial_parse.c b/shared/serial_parse.c
index 0cda2d8..0ebe377 100644
--- a/shared/serial_parse.c
+++ b/shared/serial_parse.c
@@ -1,18 +1,7 @@
#include <stdbool.h>
-#include "bin.h"
#include "consts.h"
#include "serial_parse.h"
-#ifdef W2_SIM
-#include "../robot/orangutan_shim.h"
-#endif
-
-void w2_serial_handle(w2_s_bin *code) {
-#ifdef W2_SIM
- serial_send((char *)code->data, code->bytes);
- // simprintf("yeah: %02x\n", code);
-#endif
-}
void w2_serial_parse(uint8_t byte) {
static uint8_t current_message[W2_SERIAL_READ_BUFFER_SIZE] = {0};
@@ -38,10 +27,10 @@ void w2_serial_parse(uint8_t byte) {
if (!listening) return;
current_message[current_message_index++] = byte;
+ complete_message_length = w2_scmd_length(current_message, current_message_index);
+
if (current_message_index == complete_message_length) {
- w2_s_bin *copy = w2_bin_s_alloc(current_message_index, current_message);
- w2_serial_handle(copy);
- free(copy);
+ w2_scmd_handler(current_message, current_message_index);
memset(&current_message, 0, W2_SERIAL_READ_BUFFER_SIZE);
current_message_index = 0;