fix memory leak and print sim info over stderr

5 files changed, 17 insertions, 24 deletions

diff --git a/robot/hypervisor.c b/robot/hypervisor.c
index e781877..3359bf6 100644
--- a/robot/hypervisor.c
+++ b/robot/hypervisor.c
@@ -44,9 +44,9 @@ void w2_hypervisor_main() {
 
 #ifdef W2_SIM
 	if (DBG_ENABLE_CYCLEINFO) siminfo("cycle end\n");
-	if (!g_w2_sim_headless) usleep(100e3);
+	if (DBG_CYCLE_DELAY > 0) usleep(DBG_CYCLE_DELAY);
 
-	if (g_w2_sim_headless && DBG_MAX_CYCLES > -1 && g_w2_hypervisor_cycles > DBG_MAX_CYCLES)
+	if (DBG_MAX_CYCLES > -1 && g_w2_hypervisor_cycles > DBG_MAX_CYCLES)
 		exit(0);
 #endif
 
diff --git a/robot/main.c b/robot/main.c
index eba94cb..b0302bd 100644
--- a/robot/main.c
+++ b/robot/main.c
@@ -6,9 +6,9 @@
 #endif
 
 #include <unistd.h>
-int main(int argc, char **argv) {
+int main() {
 #ifdef W2_SIM
-	w2_sim_setup(argc, argv);
+	w2_sim_setup();
 #endif
 
 	w2_setup_main();
diff --git a/robot/sim.c b/robot/sim.c
index 0e4c9b7..5956fbb 100644
--- a/robot/sim.c
+++ b/robot/sim.c
@@ -13,7 +13,6 @@
 #include "../shared/errcatch.h"
 
 struct timespec reference_time; // NOLINT
-bool g_w2_sim_headless = false;
 
 static const char* const W2_CMD_NAMES[] = {
 	"PING",
@@ -76,12 +75,10 @@ void serial_set_baud_rate(unsigned int 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);
-		fflush(stdout);
-		return;
-	}
+	for (unsigned int byte = 0; byte < length; byte++)
+		putc(message[byte] & 0xff, stdout);
+	fflush(stdout);
+	return;
 
 	simprintfunc("serial_send", "0x%02x", (uint8_t) message[0]);
 }
@@ -95,10 +92,7 @@ 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;
@@ -121,11 +115,10 @@ void w2_sim_cycle_begin() {
 }
 
 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) {
diff --git a/robot/sim.h b/robot/sim.h
index 5dbd936..9e250da 100644
--- a/robot/sim.h
+++ b/robot/sim.h
@@ -9,15 +9,14 @@
 #include "../shared/bin.h"
 #include "../shared/protocol.h"
 
-extern bool g_w2_sim_headless;
-
 // debug fine-tuning
-#define DBG_ENABLE_PRINTFUNC (1)
+#define DBG_ENABLE_PRINTFUNC (0)
 #define DBG_ENABLE_SIMWARN (1)
 #define DBG_ENABLE_SIMINFO (1)
 #define DBG_ENABLE_CYCLEINFO (0)
 #define DBG_ENABLE_SERIAL (1)
 
+#define DBG_CYCLE_DELAY (100e3)
 #define DBG_MAX_CYCLES (-1)
 
 // debug print options
@@ -47,7 +46,7 @@ extern bool g_w2_sim_headless;
 #endif
 
 // debug stdout print macros
-#define simprintf(message, ...) if (!g_w2_sim_headless) printf(COL_RED "[SIM] " COL_RST message, ##__VA_ARGS__)
+#define simprintf(message, ...) fprintf(stderr, COL_RED "[SIM] " COL_RST message, ##__VA_ARGS__)
 #define simprint(message) simprintf(message "\n")
 #define simprintfunc(name, fmt, ...) if (DBG_ENABLE_PRINTFUNC) { simprintf(COL_BLU "[FUNC] " \
 		COL_CYN name COL_RST "(" COL_YEL fmt COL_RST ")\n", ##__VA_ARGS__); }
@@ -81,7 +80,7 @@ void qtr_read(unsigned int* sensor_values, unsigned char read_mode); // NOLINT
 unsigned int analog_read(unsigned char channel); // NOLINT
 void print(const char* str); // NOLINT
 
-void w2_sim_setup(int argc, char **argv);
+void w2_sim_setup();
 void w2_sim_cycle_begin();
 void w2_sim_print_serial(w2_s_bin *data);
 
diff --git a/shared/bin.h b/shared/bin.h
index 1eaa2a7..a849d53 100644
--- a/shared/bin.h
+++ b/shared/bin.h
@@ -17,7 +17,8 @@ extern uint8_t g_w2_endianness;
 #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); \
+	w2_s_bin* bin = malloc(sizeof(w2_s_bin) + size); \
+	bin->bytes = size; \
 	type* normal = (type*) &bin->data;
 
 typedef struct {