diff --git a/quad/sw/modular_quad_pid/src/communication.c b/quad/sw/modular_quad_pid/src/communication.c
index c4e166b20bbfc56ae9b44312b23be205a4da95f2..bb86d51f18217e63d22509a215e3f629889cb074 100644
--- a/quad/sw/modular_quad_pid/src/communication.c
+++ b/quad/sw/modular_quad_pid/src/communication.c
@@ -14,6 +14,8 @@
 #define MAX_PACKET_SIZE 256
 #define UART_BUF_SIZE (((BAUD_RATE * (DESIRED_USEC_PER_LOOP / 1000) / 1000) / 10) + MAX_PACKET_SIZE + 128)
 
+#define INTERRUPT_BENCHMARK
+
 // Declaration of interrupt handler
 void Handler(void *CallBackRef, u32 Event, unsigned int EventData);
 
@@ -144,12 +146,22 @@ void uart_interrupt_handler(XUartPs *InstancePtr) {
 	u32 CsrRegister = XUartPs_ReadReg(InstancePtr->Config.BaseAddress,
 				XUARTPS_SR_OFFSET);
 
+#ifdef INTERRUPT_BENCHMARK
+		u32 start_time = timer_get_count();
+#endif
+
 	while (0 == (CsrRegister & XUARTPS_SR_RXEMPTY) && !uart_buff_full()) {
 		u8 byte = XUartPs_ReadReg(InstancePtr->Config.BaseAddress, XUARTPS_FIFO_OFFSET);
 		uart_buff_put_byte(byte);
 		CsrRegister = XUartPs_ReadReg(InstancePtr->Config.BaseAddress, XUARTPS_SR_OFFSET);
 	}
 
+#ifdef INTERRUPT_BENCHMARK
+		u32 end_time = timer_get_count();
+		u32 duration = end_time - start_time;
+		send_data(0, 0, 0, (char *) &duration, 8);
+#endif
+
 	// Clear the interrupt status.
 	XUartPs_WriteReg(InstancePtr->Config.BaseAddress, XUARTPS_ISR_OFFSET,
 			IsrStatus);
diff --git a/quad/sw/modular_quad_pid/src/main.c b/quad/sw/modular_quad_pid/src/main.c
index 43a9b9dd30fd2b6927378a4b9bb94c18e56605b4..d7c232418affae4fa7bb011b11846bd44daee859 100644
--- a/quad/sw/modular_quad_pid/src/main.c
+++ b/quad/sw/modular_quad_pid/src/main.c
@@ -17,7 +17,7 @@
 #include "update_gui.h"
 
 #define BENCH_TEST
-#define UART_BENCHMARK
+//#define UART_BENCHMARK
 
 int main()
 {