diff options
Diffstat (limited to 'robot/sim.c')
| -rw-r--r-- | robot/sim.c | 32 |
1 files changed, 15 insertions, 17 deletions
diff --git a/robot/sim.c b/robot/sim.c index 6283694..9cce12f 100644 --- a/robot/sim.c +++ b/robot/sim.c @@ -4,15 +4,15 @@ #include <stdint.h> #include <unistd.h> #include <termios.h> +#include <fcntl.h> #include "sim.h" #include "../shared/consts.h" #include "../shared/protocol.h" #include "sercomm.h" -#include "errcatch.h" +#include "../shared/errcatch.h" struct timespec reference_time; // NOLINT -bool g_w2_sim_headless = false; static const char* const W2_CMD_NAMES[] = { "PING", @@ -74,14 +74,13 @@ void serial_set_baud_rate(unsigned int rate) { simprintfunc("serial_set_baud_rate", "%u", rate); } -void serial_send(char* message, unsigned int length) { - if (g_w2_sim_headless) { - for (unsigned int byte = 0; byte < length; byte++) - putc(message[byte] & 0xff, stdout); - return; - } +void serial_send_blocking(char* message, unsigned int length) { + for (unsigned int byte = 0; byte < length; byte++) + putc(message[byte] & 0xff, stdout); + fflush(stdout); + return; - if (DBG_ENABLE_PRINTFUNC) simprintfunc("serial_send", "<%u byte%s>", length, length == 1 ? "" : "s"); + simprintfunc("serial_send", "0x%02x", (uint8_t) message[0]); } void serial_receive_ring(char* buffer, unsigned char size) { @@ -93,16 +92,14 @@ unsigned char serial_get_received_bytes() { return g_w2_serial_buffer_head; } -void w2_sim_setup(int argc, char **argv) { - if (argc > 1 && strcmp(argv[1], "headless") == 0) - g_w2_sim_headless = true; - +void w2_sim_setup() { // disable echo and enable raw mode + fcntl(STDIN_FILENO, F_SETFL, O_NONBLOCK); struct termios term; tcgetattr(STDIN_FILENO, &term); term.c_lflag &= ~(ECHO | ICANON); term.c_cc[VTIME] = 0; - term.c_cc[VMIN] = 0; + term.c_cc[VMIN] = 1; tcsetattr(STDIN_FILENO, 0, &term); // debug error @@ -110,17 +107,18 @@ void w2_sim_setup(int argc, char **argv) { } void w2_sim_cycle_begin() { + fflush(stdout); + // read bytes from stdin while(read(STDIN_FILENO, (g_w2_serial_buffer + sizeof(char) * g_w2_serial_buffer_head), 1) > 0) g_w2_serial_buffer_head = (g_w2_serial_buffer_head + 1) % W2_SERIAL_READ_BUFFER_SIZE; } void w2_sim_print_serial(w2_s_bin *data) { - if (g_w2_sim_headless) return; simprintf(COL_GRN "[%s_%s]" COL_RST, W2_CMD_NAMES[data->data[0] >> 1], W2_CMD_DIRECTIONS[data->data[0] & W2_CMD_DIRECTION_MASK]); for (int i = 0; i < data->bytes; i++) - printf(" %02x", data->data[i]); - printf("\n"); + fprintf(stderr, " %02x", data->data[i]); + fprintf(stderr, "\n"); } void set_motors(int left, int right) { |