diff options
Diffstat (limited to 'shared')
| -rw-r--r-- | shared/protocol.c | 90 | ||||
| -rw-r--r-- | shared/protocol.h | 52 | ||||
| -rw-r--r-- | shared/serial_parse.c | 4 |
3 files changed, 77 insertions, 69 deletions
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; |