diff options
Diffstat (limited to 'shared')
| -rw-r--r-- | shared/bin.c | 12 | ||||
| -rw-r--r-- | shared/bin.h | 25 | ||||
| -rw-r--r-- | shared/protocol.c | 92 | ||||
| -rw-r--r-- | shared/protocol.h | 213 | ||||
| -rw-r--r-- | shared/serial_parse.c | 17 |
5 files changed, 263 insertions, 96 deletions
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(¤t_message, 0, W2_SERIAL_READ_BUFFER_SIZE); current_message_index = 0; |