From 9cd83d662849fe72c83038a018107d9bbf5c2398 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Wed, 25 May 2022 16:30:21 +0200 Subject: WIP serial parsing --- shared/serial_parse.c | 54 +++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 54 insertions(+) create mode 100644 shared/serial_parse.c (limited to 'shared/serial_parse.c') diff --git a/shared/serial_parse.c b/shared/serial_parse.c new file mode 100644 index 0000000..c8a3fae --- /dev/null +++ b/shared/serial_parse.c @@ -0,0 +1,54 @@ +#include + +#include "consts.h" +#include "serial_parse.h" +#include "bin.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}; + static uint8_t current_message_index = 0; + static uint8_t complete_message_length = 2; + + static bool attentive = false; + static bool listening = false; + + if (byte == W2_SERIAL_START_BYTE) { + attentive = !attentive; + // if (attentive && listening) { + // current_message[current_message_index++] = byte; + // } + } else { + // activate listen after non-0xff byte after 0xff + if (attentive && !listening) { + attentive = false; + listening = true; + } + } + + if (!listening) return; + current_message[current_message_index++] = byte; + + 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); + + memset(¤t_message, 0, W2_SERIAL_READ_BUFFER_SIZE); + current_message_index = 0; + complete_message_length = 1; + attentive = false; + listening = false; + return; + } +} + -- cgit v1.2.3 From afa10fa556585d965e3a7d46c2fd5436d29cf3c6 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Wed, 25 May 2022 16:31:23 +0200 Subject: `make format` --- robot/hypervisor.c | 3 ++- shared/bin.c | 6 +++--- shared/protocol.c | 1 - shared/protocol.h | 34 ++++++++++++++++------------------ shared/serial_parse.c | 17 ++++++++--------- shared/serial_parse.h | 1 - 6 files changed, 29 insertions(+), 33 deletions(-) (limited to 'shared/serial_parse.c') diff --git a/robot/hypervisor.c b/robot/hypervisor.c index 0f754a3..c4e7aba 100644 --- a/robot/hypervisor.c +++ b/robot/hypervisor.c @@ -30,7 +30,8 @@ void w2_hypervisor_main() { if (DBG_ENABLE_CYCLEINFO) siminfo("cycle end\n"); if (!g_w2_sim_headless) usleep(100e3); - if (g_w2_sim_headless && DBG_MAX_CYCLES > -1 && g_w2_hypervisor_cycles > DBG_MAX_CYCLES) exit(0); + if (g_w2_sim_headless && DBG_MAX_CYCLES > -1 && g_w2_hypervisor_cycles > DBG_MAX_CYCLES) + exit(0); #endif g_w2_hypervisor_cycles++; diff --git a/shared/bin.c b/shared/bin.c index fc6e14b..44c2e94 100644 --- a/shared/bin.c +++ b/shared/bin.c @@ -1,5 +1,5 @@ -#include #include +#include #include "bin.h" @@ -65,8 +65,8 @@ uint32_t w2_bin_ntoh32(uint32_t n32) { return w2_bin_hton32(n32); } uint16_t w2_bin_ntoh16(uint16_t n16) { return w2_bin_hton16(n16); } w2_s_bin *w2_bin_s_alloc(uint16_t bytes, uint8_t *data) { - w2_s_bin* temp = malloc(sizeof(w2_s_bin) + sizeof(uint8_t) * bytes); - temp->bytes = bytes; + w2_s_bin *temp = malloc(sizeof(w2_s_bin) + sizeof(uint8_t) * bytes); + temp->bytes = bytes; memcpy(&temp->data, data, bytes); return temp; } diff --git a/shared/protocol.c b/shared/protocol.c index 2707725..c634a09 100644 --- a/shared/protocol.c +++ b/shared/protocol.c @@ -7,4 +7,3 @@ w2_s_cmd_ping_rx *w2_protocol_parse_cmd_ping_rx(w2_s_bin *data) { memcpy(parsed, data->data, data->bytes); return parsed; } - diff --git a/shared/protocol.h b/shared/protocol.h index b0602cf..775c2aa 100644 --- a/shared/protocol.h +++ b/shared/protocol.h @@ -1,8 +1,8 @@ #pragma once +#include #include #include -#include #include "bin.h" @@ -53,7 +53,7 @@ enum w2_e_serial_commands { // generic struct (voor parsen) // parse functie // dump functie -// +// // // als het kan deze allemaal met macro's op deze manier definieren: // @@ -74,25 +74,24 @@ enum w2_e_serial_commands { #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_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_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_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) \ -) +#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) @@ -102,4 +101,3 @@ typedef struct { } w2_s_cmd_ping_rx; w2_s_cmd_ping_rx *w2_protocol_parse_cmd_ping_rx(w2_s_bin *data); - diff --git a/shared/serial_parse.c b/shared/serial_parse.c index c8a3fae..0cda2d8 100644 --- a/shared/serial_parse.c +++ b/shared/serial_parse.c @@ -1,27 +1,27 @@ #include +#include "bin.h" #include "consts.h" #include "serial_parse.h" -#include "bin.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); + 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}; - static uint8_t current_message_index = 0; - static uint8_t complete_message_length = 2; + static uint8_t current_message_index = 0; + static uint8_t complete_message_length = 2; static bool attentive = false; static bool listening = false; - + if (byte == W2_SERIAL_START_BYTE) { attentive = !attentive; // if (attentive && listening) { @@ -44,11 +44,10 @@ void w2_serial_parse(uint8_t byte) { free(copy); memset(¤t_message, 0, W2_SERIAL_READ_BUFFER_SIZE); - current_message_index = 0; + current_message_index = 0; complete_message_length = 1; - attentive = false; - listening = false; + attentive = false; + listening = false; return; } } - diff --git a/shared/serial_parse.h b/shared/serial_parse.h index 516bf8a..a1c8fb9 100644 --- a/shared/serial_parse.h +++ b/shared/serial_parse.h @@ -6,4 +6,3 @@ /** parse serial data byte by byte */ void w2_serial_parse(uint8_t byte); - -- cgit v1.2.3 From 87900d6787f8e0af8d37cc313c4e0510ef0b1bf8 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Wed, 25 May 2022 19:02:23 +0200 Subject: serial receive working with empty handlers --- robot/sercomm.c | 61 +++++++++++++++ robot/sercomm.h | 23 +----- robot/setup.c | 2 +- robot/sim.c | 31 ++++++++ robot/sim.h | 9 ++- shared/bin.c | 12 ++- shared/bin.h | 25 +----- shared/protocol.c | 92 ++++++++++++++++++++-- shared/protocol.h | 213 ++++++++++++++++++++++++++++++++++++++------------ shared/serial_parse.c | 17 +--- 10 files changed, 363 insertions(+), 122 deletions(-) (limited to 'shared/serial_parse.c') 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 #include +#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 + #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 #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 -#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(¤t_message, 0, W2_SERIAL_READ_BUFFER_SIZE); current_message_index = 0; -- cgit v1.2.3 From e63abbdff3408c17b45c9032e7f7ba0199b43bd0 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Wed, 25 May 2022 23:36:43 +0200 Subject: struct packing and dynamic size calculation --- robot/sercomm.c | 42 +++++++++++------------ robot/sim.c | 29 +++------------- robot/tests/mcfg.bin | Bin 0 -> 11 bytes shared/protocol.c | 90 ++++++++++++++++++++++++++------------------------ shared/protocol.h | 52 +++++++++++++++-------------- shared/serial_parse.c | 4 +-- 6 files changed, 103 insertions(+), 114 deletions(-) create mode 100644 robot/tests/mcfg.bin (limited to 'shared/serial_parse.c') diff --git a/robot/sercomm.c b/robot/sercomm.c index 9e40b0e..293e3c6 100644 --- a/robot/sercomm.c +++ b/robot/sercomm.c @@ -47,8 +47,8 @@ void w2_sercomm_append_msg(w2_s_bin *data) { g_w2_sercomm_index = next_index; } -void w2_scmd_ping_rx(w2_s_bin *data) { - w2_s_cmd_ping_rx *message = malloc(w2_scmd_length(data->data, data->bytes)); +void w2_cmd_ping_rx(w2_s_bin *data) { + w2_s_cmd_ping_rx *message = malloc(w2_cmd_sizeof(data->data, data->bytes)); memcpy(message, data->data, data->bytes); size_t return_size = sizeof(w2_s_cmd_ping_tx); @@ -65,34 +65,34 @@ void w2_scmd_ping_rx(w2_s_bin *data) { free(return_message_bin); } -void w2_scmd_mode_rx(w2_s_bin *data) { return; } +void w2_cmd_mode_rx(w2_s_bin *data) { return; } -void w2_scmd_sped_rx(w2_s_bin *data) { return; } +void w2_cmd_sped_rx(w2_s_bin *data) { return; } -void w2_scmd_dirc_rx(w2_s_bin *data) { return; } +void w2_cmd_dirc_rx(w2_s_bin *data) { return; } -void w2_scmd_cord_rx(w2_s_bin *data) { return; } +void w2_cmd_cord_rx(w2_s_bin *data) { return; } -void w2_scmd_bomd_rx(w2_s_bin *data) { return; } +void w2_cmd_bomd_rx(w2_s_bin *data) { return; } -void w2_scmd_sres_rx(w2_s_bin *data) { return; } +void w2_cmd_sres_rx(w2_s_bin *data) { return; } -void w2_scmd_mcfg_rx(w2_s_bin *data) { return; } +void w2_cmd_mcfg_rx(w2_s_bin *data) { return; } -void w2_scmd_sens_rx(w2_s_bin *data) { return; } +void w2_cmd_sens_rx(w2_s_bin *data) { return; } -void w2_scmd_info_rx(w2_s_bin *data) { return; } +void w2_cmd_info_rx(w2_s_bin *data) { return; } -void w2_scmd_disp_rx(w2_s_bin *data) { return; } +void w2_cmd_disp_rx(w2_s_bin *data) { return; } -void w2_scmd_play_rx(w2_s_bin *data) { return; } +void w2_cmd_play_rx(w2_s_bin *data) { return; } -void w2_scmd_cled_rx(w2_s_bin *data) { return; } +void w2_cmd_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) {} +void w2_cmd_ping_tx(w2_s_bin *data) {} +void w2_cmd_expt_tx(w2_s_bin *data) {} +void w2_cmd_mode_tx(w2_s_bin *data) {} +void w2_cmd_cord_tx(w2_s_bin *data) {} +void w2_cmd_bomd_tx(w2_s_bin *data) {} +void w2_cmd_sens_tx(w2_s_bin *data) {} +void w2_cmd_info_tx(w2_s_bin *data) {} diff --git a/robot/sim.c b/robot/sim.c index 57e0ef5..0cde6a0 100644 --- a/robot/sim.c +++ b/robot/sim.c @@ -38,7 +38,6 @@ static const char* const W2_CMD_DIRECTIONS[] = { void time_reset() { simprintfunc("time_reset", ""); clock_gettime(CLOCK_MONOTONIC, &reference_time); - return; } unsigned long get_ms() { @@ -51,27 +50,22 @@ unsigned long get_ms() { void red_led(unsigned char on) { simprintfunc("red_led", "%i", on); - return; } void green_led(unsigned char on) { simprintfunc("green_led", "%i", on); - return; } void clear() { simprintfunc("clear", ""); - return; } void play(const char* melody) { simprintfunc("play", "\"%s\"", melody); - return; } void serial_set_baud_rate(unsigned int rate) { simprintfunc("serial_set_baud_rate", "%u", rate); - return; } void serial_send(char* message, unsigned int length) { @@ -81,25 +75,16 @@ void serial_send(char* message, unsigned int length) { return; } if (!DBG_ENABLE_PRINTFUNC) return; + simprintfunc("serial_send", ", %u", length); - unsigned int bytes = 0; - simprintf(""); - for (unsigned int byte = 0; byte < length; byte++) { - if (bytes > DBG_BYTES_PER_LINE) { - bytes = 0; - printf("\n"); - simprintf(""); - } - printf("%02x ", message[byte] & 0xff); - bytes++; - } - printf("\n"); - return; + + w2_s_bin *bin = w2_bin_s_alloc(length, (uint8_t*) message); + w2_sim_print_serial(bin); + free(bin); } void serial_receive_ring(char* buffer, unsigned char size) { simprintfunc("serial_receive_ring", "0x%016lx, %u", (unsigned long) buffer, size); - return; } unsigned char serial_get_received_bytes() { @@ -118,16 +103,12 @@ void w2_sim_setup(int argc, char **argv) { term.c_cc[VTIME] = 0; term.c_cc[VMIN] = 0; tcsetattr(STDIN_FILENO, 0, &term); - - return; } void w2_sim_cycle_begin() { // read bytes from stdin while(read(STDIN_FILENO, (g_w2_serial_buffer + sizeof(char) * g_w2_serial_buffer_head), 1) > 0) g_w2_serial_buffer_head = (g_w2_serial_buffer_head + 1) % W2_SERIAL_READ_BUFFER_SIZE; - - return; } void w2_sim_print_serial(w2_s_bin *data) { diff --git a/robot/tests/mcfg.bin b/robot/tests/mcfg.bin new file mode 100644 index 0000000..a43ecf8 Binary files /dev/null and b/robot/tests/mcfg.bin differ diff --git a/shared/protocol.c b/shared/protocol.c index 18ee227..73063f3 100644 --- a/shared/protocol.c +++ b/shared/protocol.c @@ -5,133 +5,137 @@ #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; +size_t w2_cmd_sizeof(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t data_length) { + if (data[0] == (W2_CMD_PING | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_ping_rx); + if (data[0] == (W2_CMD_PING | W2_CMDDIR_TX)) return sizeof(w2_s_cmd_ping_tx); - 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_MODE | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_mode_rx); + if (data[0] == (W2_CMD_MODE | W2_CMDDIR_TX)) return sizeof(w2_s_cmd_mode_tx); - if (data[0] == (W2_CMD_SPED | W2_CMDDIR_RX)) return 2; + if (data[0] == (W2_CMD_SPED | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_sped_rx); - if (data[0] == (W2_CMD_DIRC | W2_CMDDIR_RX)) return 5; + if (data[0] == (W2_CMD_DIRC | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_dirc_rx); - 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_CORD | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_cord_rx); + if (data[0] == (W2_CMD_CORD | W2_CMDDIR_TX)) return sizeof(w2_s_cmd_cord_tx); - 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_BOMD | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_bomd_rx); + if (data[0] == (W2_CMD_BOMD | W2_CMDDIR_TX)) return sizeof(w2_s_cmd_bomd_tx); - if (data[0] == (W2_CMD_SRES | W2_CMDDIR_RX)) return 2; + if (data[0] == (W2_CMD_SRES | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_sres_rx); - 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_SENS | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_sens_rx); + if (data[0] == (W2_CMD_SENS | W2_CMDDIR_TX)) return sizeof(w2_s_cmd_sens_tx); - if (data[0] == (W2_CMD_INFO | W2_CMDDIR_RX)) return 1; - if (data[0] == (W2_CMD_INFO | W2_CMDDIR_TX)) return 41; + if (data[0] == (W2_CMD_INFO | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_info_rx); + if (data[0] == (W2_CMD_INFO | W2_CMDDIR_TX)) return sizeof(w2_s_cmd_info_tx); w2_s_bin *copy = w2_bin_s_alloc(data_length, data); uint8_t length = 1; - 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); + if (data[0] == (W2_CMD_EXPT | W2_CMDDIR_TX)) length = w2_cmd_expt_tx_sizeof(copy); + if (data[0] == (W2_CMD_MCFG | W2_CMDDIR_RX)) length = w2_cmd_mcfg_rx_sizeof(copy); 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); +#define W2_DYN_MEMBER_SIZEOF(struct_t, length_byte, trailing_type) \ + sizeof(struct_t) + \ + (data->bytes > length_byte ? (sizeof(trailing_type) * data->data[length_byte]) : 0) + +size_t w2_cmd_expt_tx_sizeof(w2_s_bin *data) { + return W2_DYN_MEMBER_SIZEOF(w2_s_cmd_expt_tx, 2, uint8_t); } -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); +size_t w2_cmd_mcfg_rx_sizeof(w2_s_bin *data) { + return W2_DYN_MEMBER_SIZEOF(w2_s_cmd_mcfg_rx, 3, w2_s_cmd_mcfg_feature); } -void w2_scmd_handler(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t data_length) { +void w2_cmd_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); + w2_cmd_ping_rx(copy); break; } case W2_CMD_PING | W2_CMDDIR_TX: { - w2_scmd_ping_tx(copy); + w2_cmd_ping_tx(copy); break; } case W2_CMD_EXPT | W2_CMDDIR_TX: { - w2_scmd_expt_tx(copy); + w2_cmd_expt_tx(copy); break; } case W2_CMD_MODE | W2_CMDDIR_RX: { - w2_scmd_mode_rx(copy); + w2_cmd_mode_rx(copy); break; } case W2_CMD_MODE | W2_CMDDIR_TX: { - w2_scmd_mode_tx(copy); + w2_cmd_mode_tx(copy); break; } case W2_CMD_SPED | W2_CMDDIR_RX: { - w2_scmd_sped_rx(copy); + w2_cmd_sped_rx(copy); break; } case W2_CMD_DIRC | W2_CMDDIR_RX: { - w2_scmd_dirc_rx(copy); + w2_cmd_dirc_rx(copy); break; } case W2_CMD_CORD | W2_CMDDIR_RX: { - w2_scmd_cord_rx(copy); + w2_cmd_cord_rx(copy); break; } case W2_CMD_CORD | W2_CMDDIR_TX: { - w2_scmd_cord_tx(copy); + w2_cmd_cord_tx(copy); break; } case W2_CMD_BOMD | W2_CMDDIR_RX: { - w2_scmd_bomd_rx(copy); + w2_cmd_bomd_rx(copy); break; } case W2_CMD_BOMD | W2_CMDDIR_TX: { - w2_scmd_bomd_tx(copy); + w2_cmd_bomd_tx(copy); break; } case W2_CMD_SRES | W2_CMDDIR_RX: { - w2_scmd_sres_rx(copy); + w2_cmd_sres_rx(copy); break; } case W2_CMD_MCFG | W2_CMDDIR_RX: { - w2_scmd_mcfg_rx(copy); + w2_cmd_mcfg_rx(copy); break; } case W2_CMD_SENS | W2_CMDDIR_RX: { - w2_scmd_sens_rx(copy); + w2_cmd_sens_rx(copy); break; } case W2_CMD_SENS | W2_CMDDIR_TX: { - w2_scmd_sens_tx(copy); + w2_cmd_sens_tx(copy); break; } case W2_CMD_INFO | W2_CMDDIR_RX: { - w2_scmd_info_rx(copy); + w2_cmd_info_rx(copy); break; } case W2_CMD_INFO | W2_CMDDIR_TX: { - w2_scmd_info_tx(copy); + w2_cmd_info_tx(copy); break; } case W2_CMD_DISP | W2_CMDDIR_RX: { - w2_scmd_disp_rx(copy); + w2_cmd_disp_rx(copy); break; } case W2_CMD_PLAY | W2_CMDDIR_RX: { - w2_scmd_play_rx(copy); + w2_cmd_play_rx(copy); break; } case W2_CMD_CLED | W2_CMDDIR_RX: { - w2_scmd_cled_rx(copy); + w2_cmd_cled_rx(copy); break; } default: { diff --git a/shared/protocol.h b/shared/protocol.h index 52ba2bd..adec614 100644 --- a/shared/protocol.h +++ b/shared/protocol.h @@ -46,6 +46,8 @@ enum w2_e_scmds { W2_CMD_CLED = 0x1a, }; +#pragma pack(push, 1) + typedef struct { uint8_t opcode; uint8_t id; @@ -158,53 +160,55 @@ typedef struct { typedef struct { } w2_s_cmd_cled_rx; +#pragma pack(pop) + /** global handler for complete messages */ -void w2_scmd_handler(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t length); +void w2_cmd_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); +size_t w2_cmd_sizeof(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); +void w2_cmd_ping_rx(w2_s_bin *data); /** handler for ping_tx (on complete message) */ -void w2_scmd_ping_tx(w2_s_bin *data); +void w2_cmd_ping_tx(w2_s_bin *data); /** handler for expt_tx (on complete message) */ -void w2_scmd_expt_tx(w2_s_bin *data); +void w2_cmd_expt_tx(w2_s_bin *data); /** handler for mode_rx (on complete message) */ -void w2_scmd_mode_rx(w2_s_bin *data); +void w2_cmd_mode_rx(w2_s_bin *data); /** handler for mode_tx (on complete message) */ -void w2_scmd_mode_tx(w2_s_bin *data); +void w2_cmd_mode_tx(w2_s_bin *data); /** handler for sped_rx (on complete message) */ -void w2_scmd_sped_rx(w2_s_bin *data); +void w2_cmd_sped_rx(w2_s_bin *data); /** handler for dirc_rx (on complete message) */ -void w2_scmd_dirc_rx(w2_s_bin *data); +void w2_cmd_dirc_rx(w2_s_bin *data); /** handler for cord_rx (on complete message) */ -void w2_scmd_cord_rx(w2_s_bin *data); +void w2_cmd_cord_rx(w2_s_bin *data); /** handler for cord_tx (on complete message) */ -void w2_scmd_cord_tx(w2_s_bin *data); +void w2_cmd_cord_tx(w2_s_bin *data); /** handler for bomd_rx (on complete message) */ -void w2_scmd_bomd_rx(w2_s_bin *data); +void w2_cmd_bomd_rx(w2_s_bin *data); /** handler for bomd_tx (on complete message) */ -void w2_scmd_bomd_tx(w2_s_bin *data); +void w2_cmd_bomd_tx(w2_s_bin *data); /** handler for sres_rx (on complete message) */ -void w2_scmd_sres_rx(w2_s_bin *data); +void w2_cmd_sres_rx(w2_s_bin *data); /** handler for mcfg_rx (on complete message) */ -void w2_scmd_mcfg_rx(w2_s_bin *data); +void w2_cmd_mcfg_rx(w2_s_bin *data); /** handler for sens_rx (on complete message) */ -void w2_scmd_sens_rx(w2_s_bin *data); +void w2_cmd_sens_rx(w2_s_bin *data); /** handler for sens_tx (on complete message) */ -void w2_scmd_sens_tx(w2_s_bin *data); +void w2_cmd_sens_tx(w2_s_bin *data); /** handler for info_rx (on complete message) */ -void w2_scmd_info_rx(w2_s_bin *data); +void w2_cmd_info_rx(w2_s_bin *data); /** handler for info_tx (on complete message) */ -void w2_scmd_info_tx(w2_s_bin *data); +void w2_cmd_info_tx(w2_s_bin *data); /** handler for disp_rx (on complete message) */ -void w2_scmd_disp_rx(w2_s_bin *data); +void w2_cmd_disp_rx(w2_s_bin *data); /** handler for play_rx (on complete message) */ -void w2_scmd_play_rx(w2_s_bin *data); +void w2_cmd_play_rx(w2_s_bin *data); /** handler for cled_rx (on complete message) */ -void w2_scmd_cled_rx(w2_s_bin *data); +void w2_cmd_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); +size_t w2_cmd_expt_tx_sizeof(w2_s_bin *data); /** calculate message length for mcfg_rx (incomplete message) */ -uint8_t w2_scmd_mcfg_rx_length(w2_s_bin *data); +size_t w2_cmd_mcfg_rx_sizeof(w2_s_bin *data); diff --git a/shared/serial_parse.c b/shared/serial_parse.c index 0ebe377..3bf9c14 100644 --- a/shared/serial_parse.c +++ b/shared/serial_parse.c @@ -27,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); + complete_message_length = w2_cmd_sizeof(current_message, current_message_index); if (current_message_index == complete_message_length) { - w2_scmd_handler(current_message, current_message_index); + w2_cmd_handler(current_message, current_message_index); memset(¤t_message, 0, W2_SERIAL_READ_BUFFER_SIZE); current_message_index = 0; -- cgit v1.2.3 From 54b69efe150e1a102faafb4e214159c92abbb841 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Thu, 26 May 2022 12:14:04 +0200 Subject: clean up ugly switch case in favor of array lookup --- robot/setup.c | 1 + robot/tests/undefined.bin | 1 + shared/protocol.c | 125 +++++++++++++--------------------------------- shared/protocol.h | 6 +++ shared/serial_parse.c | 1 + 5 files changed, 43 insertions(+), 91 deletions(-) create mode 100644 robot/tests/undefined.bin (limited to 'shared/serial_parse.c') diff --git a/robot/setup.c b/robot/setup.c index 944ea68..f8468bf 100644 --- a/robot/setup.c +++ b/robot/setup.c @@ -25,6 +25,7 @@ void w2_setup_main() { clear(); // start serial i/o + w2_cmd_setup_handlers(); serial_set_baud_rate(W2_SERIAL_BAUD); serial_receive_ring(g_w2_serial_buffer, W2_SERIAL_READ_BUFFER_SIZE); diff --git a/robot/tests/undefined.bin b/robot/tests/undefined.bin new file mode 100644 index 0000000..d44f518 --- /dev/null +++ b/robot/tests/undefined.bin @@ -0,0 +1 @@ +ÿ€9 4­ \ No newline at end of file diff --git a/shared/protocol.c b/shared/protocol.c index 73063f3..77ec466 100644 --- a/shared/protocol.c +++ b/shared/protocol.c @@ -5,6 +5,30 @@ #include "../robot/orangutan_shim.h" #endif +void (*g_w2_cmd_handlers[W2_CMD_COUNT])(w2_s_bin *) = {0}; +void w2_cmd_setup_handlers() { + g_w2_cmd_handlers[W2_CMD_PING | W2_CMDDIR_RX] = w2_cmd_ping_rx; + g_w2_cmd_handlers[W2_CMD_PING | W2_CMDDIR_TX] = w2_cmd_ping_tx; + g_w2_cmd_handlers[W2_CMD_EXPT | W2_CMDDIR_TX] = w2_cmd_expt_tx; + g_w2_cmd_handlers[W2_CMD_MODE | W2_CMDDIR_RX] = w2_cmd_mode_rx; + g_w2_cmd_handlers[W2_CMD_MODE | W2_CMDDIR_TX] = w2_cmd_mode_tx; + g_w2_cmd_handlers[W2_CMD_SPED | W2_CMDDIR_RX] = w2_cmd_sped_rx; + g_w2_cmd_handlers[W2_CMD_DIRC | W2_CMDDIR_RX] = w2_cmd_dirc_rx; + g_w2_cmd_handlers[W2_CMD_CORD | W2_CMDDIR_RX] = w2_cmd_cord_rx; + g_w2_cmd_handlers[W2_CMD_CORD | W2_CMDDIR_TX] = w2_cmd_cord_tx; + g_w2_cmd_handlers[W2_CMD_BOMD | W2_CMDDIR_RX] = w2_cmd_bomd_rx; + g_w2_cmd_handlers[W2_CMD_BOMD | W2_CMDDIR_TX] = w2_cmd_bomd_tx; + g_w2_cmd_handlers[W2_CMD_SRES | W2_CMDDIR_RX] = w2_cmd_sres_rx; + g_w2_cmd_handlers[W2_CMD_MCFG | W2_CMDDIR_RX] = w2_cmd_mcfg_rx; + g_w2_cmd_handlers[W2_CMD_SENS | W2_CMDDIR_RX] = w2_cmd_sens_rx; + g_w2_cmd_handlers[W2_CMD_SENS | W2_CMDDIR_TX] = w2_cmd_sens_tx; + g_w2_cmd_handlers[W2_CMD_INFO | W2_CMDDIR_RX] = w2_cmd_info_rx; + g_w2_cmd_handlers[W2_CMD_INFO | W2_CMDDIR_TX] = w2_cmd_info_tx; + g_w2_cmd_handlers[W2_CMD_DISP | W2_CMDDIR_RX] = w2_cmd_disp_rx; + g_w2_cmd_handlers[W2_CMD_PLAY | W2_CMDDIR_RX] = w2_cmd_play_rx; + g_w2_cmd_handlers[W2_CMD_CLED | W2_CMDDIR_RX] = w2_cmd_cled_rx; +} + size_t w2_cmd_sizeof(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t data_length) { if (data[0] == (W2_CMD_PING | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_ping_rx); if (data[0] == (W2_CMD_PING | W2_CMDDIR_TX)) return sizeof(w2_s_cmd_ping_tx); @@ -54,101 +78,20 @@ size_t w2_cmd_mcfg_rx_sizeof(w2_s_bin *data) { } void w2_cmd_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_cmd_ping_rx(copy); - break; - } - case W2_CMD_PING | W2_CMDDIR_TX: { - w2_cmd_ping_tx(copy); - break; - } - case W2_CMD_EXPT | W2_CMDDIR_TX: { - w2_cmd_expt_tx(copy); - break; - } - case W2_CMD_MODE | W2_CMDDIR_RX: { - w2_cmd_mode_rx(copy); - break; - } - case W2_CMD_MODE | W2_CMDDIR_TX: { - w2_cmd_mode_tx(copy); - break; - } - case W2_CMD_SPED | W2_CMDDIR_RX: { - w2_cmd_sped_rx(copy); - break; - } - case W2_CMD_DIRC | W2_CMDDIR_RX: { - w2_cmd_dirc_rx(copy); - break; - } - case W2_CMD_CORD | W2_CMDDIR_RX: { - w2_cmd_cord_rx(copy); - break; - } - case W2_CMD_CORD | W2_CMDDIR_TX: { - w2_cmd_cord_tx(copy); - break; - } - case W2_CMD_BOMD | W2_CMDDIR_RX: { - w2_cmd_bomd_rx(copy); - break; - } - case W2_CMD_BOMD | W2_CMDDIR_TX: { - w2_cmd_bomd_tx(copy); - break; - } - case W2_CMD_SRES | W2_CMDDIR_RX: { - w2_cmd_sres_rx(copy); - break; - } - case W2_CMD_MCFG | W2_CMDDIR_RX: { - w2_cmd_mcfg_rx(copy); - break; - } - case W2_CMD_SENS | W2_CMDDIR_RX: { - w2_cmd_sens_rx(copy); - break; - } - case W2_CMD_SENS | W2_CMDDIR_TX: { - w2_cmd_sens_tx(copy); - break; - } - case W2_CMD_INFO | W2_CMDDIR_RX: { - w2_cmd_info_rx(copy); - break; - } - case W2_CMD_INFO | W2_CMDDIR_TX: { - w2_cmd_info_tx(copy); - break; - } - case W2_CMD_DISP | W2_CMDDIR_RX: { - w2_cmd_disp_rx(copy); - break; - } - case W2_CMD_PLAY | W2_CMDDIR_RX: { - w2_cmd_play_rx(copy); - break; - } - case W2_CMD_CLED | W2_CMDDIR_RX: { - w2_cmd_cled_rx(copy); - break; - } - default: { + w2_s_bin *copy = w2_bin_s_alloc(data_length, data); + void (*handler)(w2_s_bin *) = g_w2_cmd_handlers[data[0]]; + + if (handler == NULL) { #ifdef W2_SIM - simwarn("unknown serial message with code 0x%02x\n", data[0]); + // TODO throw warning + simwarn("unknown serial message with code 0x%02x\n", data[0]); #endif - unknown = true; - } - } - + } else { #ifdef W2_SIM - if (!unknown) w2_sim_print_serial(copy); + w2_sim_print_serial(copy); #endif + handler(copy); + } free(copy); } diff --git a/shared/protocol.h b/shared/protocol.h index adec614..5bdfcbe 100644 --- a/shared/protocol.h +++ b/shared/protocol.h @@ -15,6 +15,7 @@ #define W2_CMD_CODE_MASK (~1) #define W2_CMD_DIRECTION_MASK (1) +#define W2_CMD_COUNT 28 enum w2_e_scmds { /** ping command */ W2_CMD_PING = 0x00, @@ -162,6 +163,11 @@ typedef struct { #pragma pack(pop) +/** stores message handlers in array with opcode as index */ +extern void (*g_w2_cmd_handlers[W2_CMD_COUNT])(w2_s_bin *); +/** fills g_w2_cmd_handlers with functions */ +void w2_cmd_setup_handlers(); + /** global handler for complete messages */ void w2_cmd_handler(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t length); /** calculate message length */ diff --git a/shared/serial_parse.c b/shared/serial_parse.c index 3bf9c14..d755dc8 100644 --- a/shared/serial_parse.c +++ b/shared/serial_parse.c @@ -3,6 +3,7 @@ #include "consts.h" #include "serial_parse.h" +// TODO: give this function last time of byte, and measure if >5ms, throw warning void w2_serial_parse(uint8_t byte) { static uint8_t current_message[W2_SERIAL_READ_BUFFER_SIZE] = {0}; static uint8_t current_message_index = 0; -- cgit v1.2.3 From fda1e9389c4bcef89a2bcdd2c01f0727e9efdb18 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Thu, 26 May 2022 14:45:36 +0200 Subject: add semver parsing, more error codes and verify avr compilation compatibility --- shared/bin.c | 2 +- shared/errors.h | 8 ++++++++ shared/protocol.h | 1 - shared/semver.c | 9 +++++++++ shared/semver.h | 11 +++++++++++ shared/serial_parse.c | 1 + 6 files changed, 30 insertions(+), 2 deletions(-) create mode 100644 shared/semver.c create mode 100644 shared/semver.h (limited to 'shared/serial_parse.c') diff --git a/shared/bin.c b/shared/bin.c index e9592e6..4b3dcc6 100644 --- a/shared/bin.c +++ b/shared/bin.c @@ -1,5 +1,5 @@ -#include #include +#include #include "bin.h" diff --git a/shared/errors.h b/shared/errors.h index 985fcb1..44b29a0 100644 --- a/shared/errors.h +++ b/shared/errors.h @@ -24,6 +24,8 @@ typedef enum { W2_E_CRIT_LINE_LOST = 0x02 | W2_E_TYPE_CRIT, /** obstacle unavoidable, robot stuck */ W2_E_CRIT_OBSTACLE_STUCK = 0x03 | W2_E_TYPE_CRIT, + /** semver major version doesn't match */ + W2_E_CRIT_VERSION_INCOMPATIBLE = 0x04 | W2_E_TYPE_CRIT, /** battery low, returning to charging station */ W2_E_WARN_BATTERY_LOW = 0x00 | W2_E_TYPE_WARN, @@ -39,6 +41,12 @@ typedef enum { W2_E_WARN_LINE_LOST = 0x05 | W2_E_TYPE_WARN, /** serial buffer full, gets thrown on next cycle */ W2_E_WARN_SERCOMM_BUFFER_FULL = 0x06 | W2_E_TYPE_WARN, + /** semver minor version doesn't match */ + W2_E_WARN_VERSION_INCOMPATIBLE = 0x07 | W2_E_TYPE_WARN, + /** serial byte took to long to receive */ + W2_E_WARN_SERIAL_TIMEOUT = 0x08 | W2_E_TYPE_WARN, + /** unknown message encountered (noisy channel?) */ + W2_E_WARN_SERIAL_NOISY = 0x09 | W2_E_TYPE_WARN, } w2_e_errorcode; /** diff --git a/shared/protocol.h b/shared/protocol.h index 875bb16..057f67d 100644 --- a/shared/protocol.h +++ b/shared/protocol.h @@ -1,6 +1,5 @@ #pragma once -#include #include #include diff --git a/shared/semver.c b/shared/semver.c new file mode 100644 index 0000000..07a7057 --- /dev/null +++ b/shared/semver.c @@ -0,0 +1,9 @@ +#include "semver.h" + +#include + +w2_semver_t w2_semver_parse(const char *str, uint8_t length) { + w2_semver_t version; + sscanf(str, "%d.%d.%d", (int *)&version.major, (int *)&version.minor, (int *)&version.patch); + return version; +} diff --git a/shared/semver.h b/shared/semver.h new file mode 100644 index 0000000..a99ae61 --- /dev/null +++ b/shared/semver.h @@ -0,0 +1,11 @@ +#pragma once + +#include + +typedef struct { + uint8_t major; + uint8_t minor; + uint8_t patch; +} w2_semver_t; + +w2_semver_t w2_semver_parse(const char *str, uint8_t length); diff --git a/shared/serial_parse.c b/shared/serial_parse.c index d755dc8..89c5809 100644 --- a/shared/serial_parse.c +++ b/shared/serial_parse.c @@ -1,4 +1,5 @@ #include +#include #include "consts.h" #include "serial_parse.h" -- cgit v1.2.3