diff options
| -rw-r--r-- | robot/sercomm.c | 108 | ||||
| -rw-r--r-- | shared/bin.h | 7 |
2 files changed, 49 insertions, 66 deletions
diff --git a/robot/sercomm.c b/robot/sercomm.c index e346cc2..5ac9a87 100644 --- a/robot/sercomm.c +++ b/robot/sercomm.c @@ -58,39 +58,32 @@ void w2_sercomm_append_msg(w2_s_bin *data) { g_w2_sercomm_index = next_index; } -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); - w2_s_cmd_ping_tx *return_message = malloc(return_size); - return_message->opcode = W2_CMD_PING | W2_CMDDIR_TX; - return_message->id = message->id; +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Warray-bounds" - w2_s_bin *return_message_bin = w2_bin_s_alloc(return_size, (uint8_t *)return_message); +void w2_cmd_ping_rx(w2_s_bin *data) { + W2_CAST_BIN(w2_s_cmd_ping_rx, data, req); - w2_sercomm_append_msg(return_message_bin); + W2_CREATE_MSG_BIN(w2_s_cmd_ping_tx, res_msg, res_bin); + res_msg->opcode = W2_CMD_PING | W2_CMDDIR_TX; + res_msg->id = req->id; - free(message); - free(return_message); - free(return_message_bin); + w2_sercomm_append_msg(res_bin); + free(res_bin); } void w2_cmd_mode_rx(w2_s_bin *data) { - w2_s_cmd_mode_rx *message = malloc(w2_cmd_sizeof(data->data, data->bytes)); - memcpy(message, data->data, data->bytes); - - w2_modes_swap(message->mode); + W2_CAST_BIN(w2_s_cmd_mode_rx, data, req); + w2_modes_swap(req->mode); } void w2_cmd_sped_rx(w2_s_bin *data) { return; } void w2_cmd_dirc_rx(w2_s_bin *data) { - w2_s_cmd_dirc_rx *message = malloc(w2_cmd_sizeof(data->data, data->bytes)); - memcpy(message, data->data, data->bytes); + W2_CAST_BIN(w2_s_cmd_dirc_rx, data, req); - g_w2_mode_dirc_motor_l = w2_bin_ntoh16(message->left); - g_w2_mode_dirc_motor_r = w2_bin_ntoh16(message->right); + g_w2_mode_dirc_motor_l = w2_bin_ntoh16(req->left); + g_w2_mode_dirc_motor_r = w2_bin_ntoh16(req->right); } void w2_cmd_cord_rx(w2_s_bin *data) { return; } @@ -98,10 +91,9 @@ void w2_cmd_cord_rx(w2_s_bin *data) { return; } void w2_cmd_bomd_rx(w2_s_bin *data) { return; } void w2_cmd_sres_rx(w2_s_bin *data) { - w2_s_cmd_sres_rx *message = malloc(w2_cmd_sizeof(data->data, data->bytes)); - memcpy(message, data->data, data->bytes); + W2_CAST_BIN(w2_s_cmd_sres_rx, data, req); - switch (message->type) { + switch (req->type) { case W2_CMD_SRES_RX_TYPE_REINITGS: { // TODO: soft-reset break; @@ -119,51 +111,33 @@ void w2_cmd_sres_rx(w2_s_bin *data) { void w2_cmd_mcfg_rx(w2_s_bin *data) { return; } void w2_cmd_sens_rx(w2_s_bin *data) { - w2_s_cmd_sens_rx *message = malloc(w2_cmd_sizeof(data->data, data->bytes)); - memcpy(message, data->data, data->bytes); - - size_t return_size = sizeof(w2_s_cmd_sens_tx); - w2_s_cmd_sens_tx *return_message = malloc(return_size); - return_message->opcode = W2_CMD_SENS | W2_CMDDIR_TX; - memcpy((uint8_t *)&return_message->io, (uint8_t *)&g_w2_io, sizeof(w2_s_io_all)); - - for (int i = 0; i < 5; i++) w2_bin_repl_hton16(&return_message->io.qtr[i].range); - w2_bin_repl_hton16(&return_message->io.front_distance.detection); - w2_bin_repl_hton16(&return_message->io.side_distance.detection); - w2_bin_repl_hton16(&return_message->io.battery.charge_level); - w2_bin_repl_hton16((uint16_t *)&return_message->io.motor_left.speed); - w2_bin_repl_hton16((uint16_t *)&return_message->io.motor_right.speed); - - w2_s_bin *return_message_bin = w2_bin_s_alloc(return_size, (uint8_t *)return_message); - - w2_sercomm_append_msg(return_message_bin); - - free(message); - free(return_message); - free(return_message_bin); + W2_CREATE_MSG_BIN(w2_s_cmd_sens_tx, res_msg, res_bin); + res_msg->opcode = W2_CMD_SENS | W2_CMDDIR_TX; + memcpy((uint8_t *)&res_msg->io, (uint8_t *)&g_w2_io, sizeof(w2_s_io_all)); + + for (int i = 0; i < 5; i++) w2_bin_repl_hton16(&res_msg->io.qtr[i].range); + w2_bin_repl_hton16(&res_msg->io.front_distance.detection); + w2_bin_repl_hton16(&res_msg->io.side_distance.detection); + w2_bin_repl_hton16(&res_msg->io.battery.charge_level); + w2_bin_repl_hton16((uint16_t *)&res_msg->io.motor_left.speed); + w2_bin_repl_hton16((uint16_t *)&res_msg->io.motor_right.speed); + + w2_sercomm_append_msg(res_bin); + free(res_bin); } void w2_cmd_info_rx(w2_s_bin *data) { - w2_s_cmd_info_rx *message = malloc(w2_cmd_sizeof(data->data, data->bytes)); - memcpy(message, data->data, data->bytes); - - size_t return_size = sizeof(w2_s_cmd_info_tx); - w2_s_cmd_info_tx *return_message = malloc(return_size); - return_message->opcode = W2_CMD_INFO | W2_CMDDIR_TX; - strncpy((char *)return_message->build_str, W2_BUILD_STR, sizeof(return_message->build_str)); - return_message->errcatch_ms = (uint8_t)g_w2_hypervisor_ema_errcatch_ms; - return_message->io_ms = (uint8_t)g_w2_hypervisor_ema_io_ms; - return_message->sercomm_ms = (uint8_t)g_w2_hypervisor_ema_sercomm_ms; - return_message->mode_ms = (uint8_t)g_w2_hypervisor_ema_mode_ms; - return_message->uptime_s = w2_bin_hton32((uint32_t)(g_w2_hypervisor_uptime_ms / 1e3)); - - w2_s_bin *return_message_bin = w2_bin_s_alloc(return_size, (uint8_t *)return_message); - - w2_sercomm_append_msg(return_message_bin); - - free(message); - free(return_message); - free(return_message_bin); + W2_CREATE_MSG_BIN(w2_s_cmd_info_tx, res_msg, res_bin); + res_msg->opcode = W2_CMD_INFO | W2_CMDDIR_TX; + strncpy((char *)res_msg->build_str, W2_BUILD_STR, sizeof(res_msg->build_str)); + res_msg->errcatch_ms = (uint8_t)g_w2_hypervisor_ema_errcatch_ms; + res_msg->io_ms = (uint8_t)g_w2_hypervisor_ema_io_ms; + res_msg->sercomm_ms = (uint8_t)g_w2_hypervisor_ema_sercomm_ms; + res_msg->mode_ms = (uint8_t)g_w2_hypervisor_ema_mode_ms; + res_msg->uptime_s = w2_bin_hton32((uint32_t)(g_w2_hypervisor_uptime_ms / 1e3)); + + w2_sercomm_append_msg(res_bin); + free(res_bin); } void w2_cmd_disp_rx(w2_s_bin *data) { return; } @@ -172,6 +146,8 @@ void w2_cmd_play_rx(w2_s_bin *data) { return; } void w2_cmd_cled_rx(w2_s_bin *data) { return; } +#pragma GCC diagnostic pop + 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) {} diff --git a/shared/bin.h b/shared/bin.h index 48485c8..1eaa2a7 100644 --- a/shared/bin.h +++ b/shared/bin.h @@ -13,6 +13,13 @@ extern uint8_t g_w2_endianness; +#define W2_CAST_BIN(type, in, out) type *out = (type*) &in->data; +#define W2_CREATE_MSG_BIN(type, normal, bin) \ + W2_CREATE_MSG_SIZE_BIN(type, sizeof(type), normal, bin) +#define W2_CREATE_MSG_SIZE_BIN(type, size, normal, bin) \ + w2_s_bin* bin = malloc(size); \ + type* normal = (type*) &bin->data; + typedef struct { uint16_t bytes; uint8_t data[]; |