diff options
Diffstat (limited to 'robot/sercomm.c')
| -rw-r--r-- | robot/sercomm.c | 163 |
1 files changed, 72 insertions, 91 deletions
diff --git a/robot/sercomm.c b/robot/sercomm.c index 07c4bd8..c50dd15 100644 --- a/robot/sercomm.c +++ b/robot/sercomm.c @@ -2,8 +2,8 @@ #include <string.h> #include "../shared/bin.h" +#include "../shared/errcatch.h" #include "../shared/serial_parse.h" -#include "errcatch.h" #include "hypervisor.h" #include "io.h" #include "mode_dirc.h" @@ -31,16 +31,40 @@ void w2_sercomm_main() { g_w2_serial_buffer_index = (g_w2_serial_buffer_index + 1) % W2_SERIAL_READ_BUFFER_SIZE; } + // check time-out + if (!g_w2_ping_received && w2_hypervisor_time_end(W2_TIMER_PING) > W2_PING_TIMEOUT) { + g_w2_ping_timeout = true; + g_w2_connected = false; + w2_errcatch_throw(W2_E_WARN_PING_TIMEOUT); + } + // send ping every W2_TIMER_PING ms + if ((g_w2_ping_received && w2_hypervisor_time_end(W2_TIMER_PING) > W2_PING_FREQUENCY) || + g_w2_ping_timeout) { + g_w2_ping_timeout = false; + g_w2_ping_received = false; + g_w2_ping_id = (uint8_t)rand(); + + W2_CREATE_MSG_BIN(w2_s_cmd_ping_tx, msg, bin); + msg->opcode = W2_CMD_PING | W2_CMDDIR_TX; + msg->id = g_w2_ping_id; + + w2_sercomm_append_msg(bin); + free(bin); + + w2_hypervisor_time_start(W2_TIMER_PING); + } + // send data while (g_w2_sercomm_offset != g_w2_sercomm_index) { w2_s_bin *data = g_w2_sercomm_buffer[g_w2_sercomm_offset]; #ifdef W2_SIM - w2_sim_print_serial(data); + if (DBG_ENABLE_SERIAL) w2_sim_print_serial(data); #endif - serial_send("\xff", 1); + serial_send_blocking("\xff", 1); for (uint8_t i = 0; i < data->bytes; i++) { uint8_t byte = data->data[i]; - byte == 0xff ? serial_send("\xff\xff", 2) : serial_send((char *)&byte, 1); + byte == 0xff ? serial_send_blocking("\xff\xff", 2) + : serial_send_blocking((char *)&byte, 1); } g_w2_sercomm_offset = (g_w2_sercomm_offset + 1) % W2_SERCOMM_BUFFER_SIZE; } @@ -58,59 +82,33 @@ void w2_sercomm_append_msg(w2_s_bin *data) { g_w2_sercomm_index = next_index; } -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); - void (*handler)(w2_s_bin *) = g_w2_cmd_handlers[data[0]]; +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Warray-bounds" - if (handler == NULL) { -#ifdef W2_SIM - // TODO throw warning - simwarn("unknown serial message with code 0x%02x\n", data[0]); -#endif - w2_errcatch_throw(W2_E_WARN_SERIAL_NOISY); - } else { -#ifdef W2_SIM - w2_sim_print_serial(copy); -#endif - handler(copy); - } +#include <stdlib.h> +#include <string.h> - free(copy); +void w2_cmd_ping_tx(w2_s_bin *data) { + g_w2_ping_ms = w2_hypervisor_time_end(W2_TIMER_PING); + g_w2_ping_received = true; + g_w2_ping_timeout = false; + g_w2_connected = true; } -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; - - 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); -} +void w2_cmd_ping_rx(w2_s_bin *data) { w2_sercomm_append_msg(data); } 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; } @@ -118,10 +116,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; @@ -139,51 +136,34 @@ 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)); + res_msg->mode = g_w2_mode_history[g_w2_mode_history_index]; + + w2_sercomm_append_msg(res_bin); + free(res_bin); } void w2_cmd_disp_rx(w2_s_bin *data) { return; } @@ -192,7 +172,8 @@ void w2_cmd_play_rx(w2_s_bin *data) { return; } void w2_cmd_cled_rx(w2_s_bin *data) { return; } -void w2_cmd_ping_tx(w2_s_bin *data) {} +#pragma GCC diagnostic pop + 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) {} |