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) {