diff --git a/quad/xsdk_workspace_vivado/real_quad/src/hw_impl_zybo.c b/quad/xsdk_workspace_vivado/real_quad/src/hw_impl_zybo.c
index 4ebdef1857185c22c13531e6581fc4fe7c0192a7..df4cd13a3180ba7fe76c125cc8a72ce89e9aaa8c 100644
--- a/quad/xsdk_workspace_vivado/real_quad/src/hw_impl_zybo.c
+++ b/quad/xsdk_workspace_vivado/real_quad/src/hw_impl_zybo.c
@@ -65,6 +65,15 @@ struct TimerDriver create_zybo_global_timer() {
   return global_timer;
 }
 
+struct TimerDriver create_zybo_axi_timer() {
+  struct TimerDriver axi_timer;
+  axi_timer.state = NULL;
+  axi_timer.reset = zybo_axi_timer_reset;
+  axi_timer.restart = zybo_axi_timer_restart;
+  axi_timer.read = zybo_axi_timer_read;
+  return axi_timer;
+}
+
 struct LEDDriver create_zybo_mio7_led() {
   struct LEDDriver mio7_led;
   mio7_led.state = NULL;
diff --git a/quad/xsdk_workspace_vivado/real_quad/src/hw_impl_zybo.h b/quad/xsdk_workspace_vivado/real_quad/src/hw_impl_zybo.h
index d8738b71c248dd0b678837b3edac8e7d1d2b227f..8f439cdf1872f1da5bcd0638d76a152f58c27f8f 100644
--- a/quad/xsdk_workspace_vivado/real_quad/src/hw_impl_zybo.h
+++ b/quad/xsdk_workspace_vivado/real_quad/src/hw_impl_zybo.h
@@ -5,6 +5,7 @@
 
 #include <sleep.h>
 #include <stdlib.h>
+#include <xtmrctr.h>
 #include <xgpiops.h>
 #include "xiicps.h"
 #include "xparameters.h"
diff --git a/quad/xsdk_workspace_vivado/real_quad/src/hw_impl_zybo_axi_timer.c b/quad/xsdk_workspace_vivado/real_quad/src/hw_impl_zybo_axi_timer.c
new file mode 100644
index 0000000000000000000000000000000000000000..fa4bb05a6a5bb0bea2167446297a9a29c0b6b9ca
--- /dev/null
+++ b/quad/xsdk_workspace_vivado/real_quad/src/hw_impl_zybo_axi_timer.c
@@ -0,0 +1,22 @@
+#include "hw_impl_zybo.h"
+
+#define PL_CLK_CNTS_PER_USEC 100
+
+int zybo_axi_timer_reset(struct TimerDriver *self) {
+  if (self->state == NULL) {
+    self->state = malloc(sizeof(XTmrCtr));
+  }
+  return XTmrCtr_Initialize(self->state, XPAR_AXI_TIMER_0_DEVICE_ID);
+}
+
+int zybo_axi_timer_restart(struct TimerDriver *self) {
+  XTmrCtr_Reset(self->state, 0);
+  XTmrCtr_Start(self->state, 0);
+  return 0;
+}
+
+int zybo_axi_timer_read(struct TimerDriver *self, u64 *us) {
+  // Returns the number of useconds
+  *us = XTmrCtr_GetValue(self->state, 0) / PL_CLK_CNTS_PER_USEC;
+  return 0;
+}
diff --git a/quad/xsdk_workspace_vivado/real_quad/src/hw_impl_zybo_tests.c b/quad/xsdk_workspace_vivado/real_quad/src/hw_impl_zybo_tests.c
index f7d1bb2f1316558e683a881fa247465d5a77f650..a38e87306620ba54d308d61cd450cf7e817614cc 100644
--- a/quad/xsdk_workspace_vivado/real_quad/src/hw_impl_zybo_tests.c
+++ b/quad/xsdk_workspace_vivado/real_quad/src/hw_impl_zybo_tests.c
@@ -209,6 +209,41 @@ int test_zybo_motors() {
   return 0;
 }
 
+/**
+ * Test for the AXI timer, using LEDDriver.
+ *
+ * This is essentially a basic "blink" program, using the mio7 LED
+ * on the Zybo board.
+ *
+ * Instructions:
+ * 1) Connect Zybo board to computer by USB cable.
+ * 2) Set the RUN_TESTS macro in main.c
+ * 3) Uncomment only this test in main.c
+ * 4) Run main.c
+ * 5) Observe MIO7 LED on board blinking at 1 second intervals.
+ */
+int test_zybo_axi_timer() {
+  struct TimerDriver axi = create_zybo_axi_timer();
+  struct LEDDriver led = create_zybo_mio7_led();
+  axi.reset(&axi);
+  led.reset(&led);
+
+  unsigned long time;
+
+  while (1) {
+    axi.restart(&axi);
+    time = 0;
+    while (time < 1000000) {
+      axi.read(&axi, &time);
+    }
+    led.turn_off(&led);
+    while (time < 2000000) {
+      axi.read(&axi, &time);
+    }
+    led.turn_on(&led);
+  }
+}
+
 /**
  * Test for the Global timer, using LEDDriver.
  *
diff --git a/quad/xsdk_workspace_vivado/real_quad/src/main.c b/quad/xsdk_workspace_vivado/real_quad/src/main.c
index 30d3132df9de189d396ebf25470f2f0aeeeb4648..f6991aca7623d2a99bdba5eb5a048e424874b5ff 100644
--- a/quad/xsdk_workspace_vivado/real_quad/src/main.c
+++ b/quad/xsdk_workspace_vivado/real_quad/src/main.c
@@ -4,7 +4,7 @@
 #include "type_def.h"
 #include "platform.h"
 
-#define RUN_TESTS
+//#define RUN_TESTS
 
 /**
  * Create the hardware drivers, and place them on the hardware struct.
@@ -29,6 +29,7 @@ int setup_hardware(hardware_t *hardware) {
   hardware->comm = create_zybo_comm(&hardware->uart_0);
   hardware->gps = create_zybo_gps(&hardware->uart_1);
   hardware->global_timer = create_zybo_global_timer();
+  hardware->axi_timer = create_zybo_axi_timer();
   hardware->mio7_led = create_zybo_mio7_led();
   hardware->sys = create_zybo_system();
   hardware->i2c_0 = create_zybo_i2c(0);
@@ -47,8 +48,8 @@ int main()
 #ifdef RUN_TESTS
   //test_zybo_mio7_led_and_system();
   //test_zybo_i2c();
-  test_zybo_i2c_imu();
-  //test_zybo_i2c_px4flow();
+  //test_zybo_i2c_imu();
+  test_zybo_i2c_px4flow();
   //test_zybo_i2c_lidar();
   //test_zybo_i2c_all();
   //test_zybo_rc_receiver();