diff --git a/quad/scripts/test_uart_comm.py b/quad/scripts/test_uart_comm.py
index 1a28e3ba668d40de3c69b8d1a25baf4777456f21..324f73b1f7dde43b0701ee57ab19c759751cf41a 100755
--- a/quad/scripts/test_uart_comm.py
+++ b/quad/scripts/test_uart_comm.py
@@ -1,6 +1,7 @@
 #!/usr/bin/python
 
 import sys
+import time
 
 print(sys.version_info)
 import serial
@@ -44,7 +45,13 @@ def query_received(ser):
 
 if __name__ == '__main__':
     with serial.Serial('/dev/ttyUSB0', 921600, timeout=5) as ser:
-        ser.write(create_test_packet(8))
+        ser.reset_input_buffer()
+        ser.write(create_test_packet(40))
+        time.sleep(0.05)
+        #while ser.in_waiting != 0:
+        #    resp = read_packet(ser)
+        #    elapsed = int.from_bytes(resp, byteorder='little')
+        #    print(f"Took {elapsed} cycles, {elapsed / 100} us")
         ser.flush()
-        print(query_received(ser))
+        #print(query_received(ser))
 
diff --git a/quad/sw/modular_quad_pid/src/initialize_components.c b/quad/sw/modular_quad_pid/src/initialize_components.c
index fac31e173056af4691962e3bc8ad5dd54602de0c..e00a27b8cb45265863b68d4b2e2e8879a88651bc 100644
--- a/quad/sw/modular_quad_pid/src/initialize_components.c
+++ b/quad/sw/modular_quad_pid/src/initialize_components.c
@@ -56,6 +56,12 @@ int initializeAllComponents(user_input_t * user_input_struct, log_t * log_struct
 	// Xilinx given initialization
 	init_platform();
 
+
+	// Initialize loop timers
+	if (timer_init()) {
+		return -1;
+	}
+
 	// Initialize UART0 (Bluetooth)
 	if (initUartComms()) {
 		return -1;
@@ -69,11 +75,6 @@ int initializeAllComponents(user_input_t * user_input_struct, log_t * log_struct
 	// Initialize PWM Recorders and Motor outputs
 	pwm_init();
 
-	// Initialize loop timers
-	if (timer_init()) {
-		return -1;
-	}
-
 	//manual flight mode
 	user_defined_struct->flight_mode = MANUAL_FLIGHT_MODE;
 
diff --git a/quad/sw/modular_quad_pid/src/timer.c b/quad/sw/modular_quad_pid/src/timer.c
index 7d5f3b11bb2c28c42deabc3ea56209f39c84afb7..2c39ed1a7a484f8bdd2715dcf1c519f3afaa7581 100644
--- a/quad/sw/modular_quad_pid/src/timer.c
+++ b/quad/sw/modular_quad_pid/src/timer.c
@@ -64,3 +64,7 @@ int timer_end_loop(log_t *log_struct)
 
 	return 0;
 }
+
+u32 timer_get_count() {
+	return XTmrCtr_GetValue(&axi_timer, 0);
+}
diff --git a/quad/sw/modular_quad_pid/src/timer.h b/quad/sw/modular_quad_pid/src/timer.h
index dbd7190276da814c5a4b20f6e3b2713d19103778..64e067e9a3779affbb205733d6885bc4bd71eaf2 100644
--- a/quad/sw/modular_quad_pid/src/timer.h
+++ b/quad/sw/modular_quad_pid/src/timer.h
@@ -53,4 +53,6 @@ int timer_start_loop();
  */
 int timer_end_loop(log_t *log_struct);
 
+u32 timer_get_count();
+
 #endif /* TIMER_H_ */