diff options
Diffstat (limited to 'shared')
| -rw-r--r-- | shared/protocol.c | 125 | ||||
| -rw-r--r-- | shared/protocol.h | 6 | ||||
| -rw-r--r-- | shared/serial_parse.c | 1 |
3 files changed, 41 insertions, 91 deletions
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; |