remove windows sim compatibility and fix 0xff byte handling

1 files changed, 10 insertions, 5 deletions

diff --git a/robot/sercomm.c b/robot/sercomm.c
index 83d6419..e0ea39d 100644
--- a/robot/sercomm.c
+++ b/robot/sercomm.c
@@ -26,16 +26,21 @@ void w2_sercomm_main() {
 #endif
 	// read and parse data
 	while (serial_get_received_bytes() != g_w2_serial_buffer_index) {
-		w2_serial_parse(g_w2_serial_buffer[g_w2_serial_buffer_index]);
+		if (!w2_serial_parse(g_w2_serial_buffer[g_w2_serial_buffer_index]))
+			w2_errcatch_throw(W2_E_WARN_SERIAL_NOISY);
 		g_w2_serial_buffer_index = (g_w2_serial_buffer_index + 1) % W2_SERIAL_READ_BUFFER_SIZE;
 	}
 
 	// send data
 	while (g_w2_sercomm_offset != g_w2_sercomm_index) {
-		w2_s_bin *data	= g_w2_sercomm_buffer[g_w2_sercomm_offset];
-		char *data_cast = malloc(data->bytes);
-		memcpy(data_cast, data->data, data->bytes);
-		serial_send(data_cast, data->bytes);
+		w2_s_bin *data = g_w2_sercomm_buffer[g_w2_sercomm_offset];
+#ifdef W2_SIM
+		w2_sim_print_serial(data);
+#endif
+		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);
+		}
 		g_w2_sercomm_offset = (g_w2_sercomm_offset + 1) % W2_SERCOMM_BUFFER_SIZE;
 	}
 }