diff --git a/quad/src/quad_app/callbacks.c b/quad/src/quad_app/callbacks.c
index cb99d83ad9d108e297121666df94159e96fbaedd..8c06e3bdd5283a234578398a2246a1a322291ba5 100644
--- a/quad/src/quad_app/callbacks.c
+++ b/quad/src/quad_app/callbacks.c
@@ -28,7 +28,7 @@ int cb_debug(struct modular_structs *structs, struct metadata *meta, unsigned ch
 	float param_val = graph_get_output(graph, node_id, 0);
 
 	int len = snprintf((char*)buf, sizeof buf, "%f", param_val);
-	send_data(&structs->hardware_struct.uart, DEBUG_ID, 0, buf, len >= sizeof(buf) ? 255 : length + 1);
+	send_data(structs->hardware_struct.comm.uart, DEBUG_ID, 0, buf, len >= sizeof(buf) ? 255 : length + 1);
 	return 0;
 }
 
@@ -40,7 +40,7 @@ int cb_packetlog(struct modular_structs* structs, struct metadata *meta, u8 *dat
 	n_msg_received += 1;
 	total_payload_received += length;
 	int len = sprintf(buf, "Packets received: %d", n_msg_received);
-	send_data(&structs->hardware_struct.uart, PACKETLOG_ID, 0, buf, len);
+	send_data(structs->hardware_struct.comm.uart, PACKETLOG_ID, 0, buf, len);
 	return 0;
 }
 
@@ -54,7 +54,7 @@ int cb_getpacketlogs(struct modular_structs* structs, struct metadata *meta, u8
 	// Message logging number of messages received and size of payload received
 	int len = snprintf((char*)buf, sizeof buf, "%d,%lu", n_msg_received, total_payload_received);
 
-	send_data(&structs->hardware_struct.uart, LOG_ID, 0, buf, len >= sizeof(buf) ? 255 : len + 1);
+	send_data(structs->hardware_struct.comm.uart, LOG_ID, 0, buf, len >= sizeof(buf) ? 255 : len + 1);
 	return 0;
 }
 
@@ -202,7 +202,7 @@ int cb_getparam(struct modular_structs *structs, struct metadata *meta, unsigned
 	pack_float(param_val, resp_data + 4);
 
 	// Send the response
-	send_data(&structs->hardware_struct.uart, RESPPARAM_ID, msg_id, resp_data, sizeof(resp_data));
+	send_data(structs->hardware_struct.comm.uart, RESPPARAM_ID, msg_id, resp_data, sizeof(resp_data));
 
 	return 5;
 }
@@ -274,7 +274,7 @@ int cb_getsource(struct modular_structs *structs, struct metadata *meta, unsigne
 	pack_short(source.controller_id, resp_data + 4);
 	pack_short(source.controller_output, resp_data + 6);
 
-	send_data(&structs->hardware_struct.uart, RESPSOURCE_ID, msg_id, resp_data, sizeof(resp_data));
+	send_data(structs->hardware_struct.comm.uart, RESPSOURCE_ID, msg_id, resp_data, sizeof(resp_data));
 	return 0;
 }
 
@@ -303,7 +303,7 @@ int cb_getoutput(struct modular_structs *structs, struct metadata *meta, unsigne
 	pack_float(output_val, resp_data + 4);
 
 	// Send the response
-	send_data(&structs->hardware_struct.uart, RESPOUTPUT_ID, msg_id, resp_data, sizeof(resp_data));
+	send_data(structs->hardware_struct.comm.uart, RESPOUTPUT_ID, msg_id, resp_data, sizeof(resp_data));
 
 	return 0;
 }
@@ -325,7 +325,7 @@ int cb_getnodes(struct modular_structs *structs, struct metadata *meta, unsigned
 	const struct computation_graph* graph = structs->parameter_struct.graph;
 	if (graph->n_nodes >= 150) {
 		static char* error_msg = "Over 150 nodes. Not responding to cb_getnodes for fear of buffer overflow.";
-		send_data(&structs->hardware_struct.uart, DEBUG_ID, 0,
+		send_data(structs->hardware_struct.comm.uart, DEBUG_ID, 0,
 				(u8*)error_msg, sizeof(error_msg));
 		return -1;
 	}
@@ -369,7 +369,7 @@ int cb_getnodes(struct modular_structs *structs, struct metadata *meta, unsigned
 		}
 	}
 
-	send_data(&structs->hardware_struct.uart, RESPNODES_ID, meta->msg_id, resp_buf, offset);
+	send_data(structs->hardware_struct.comm.uart, RESPNODES_ID, meta->msg_id, resp_buf, offset);
 	return 0;
 }
 
@@ -406,6 +406,6 @@ int cb_addnode(struct modular_structs *structs, struct metadata *meta, unsigned
 	u8 resp_buf[2];
 	pack_short(new_node_id, resp_buf);
 
-	send_data(&structs->hardware_struct.uart, RESPADDNODE_ID, meta->msg_id, resp_buf, sizeof(resp_buf));
+	send_data(structs->hardware_struct.comm.uart, RESPADDNODE_ID, meta->msg_id, resp_buf, sizeof(resp_buf));
 	return 0;
 }
diff --git a/quad/src/quad_app/communication.c b/quad/src/quad_app/communication.c
index 210ae1c6bc7f95cabc234be1aeae90e8c4722248..2f0183c984d3ad1d38ff8a07d3f72a8427675481 100644
--- a/quad/src/quad_app/communication.c
+++ b/quad/src/quad_app/communication.c
@@ -96,7 +96,7 @@ int process_packet(modular_structs_t *structs) {
 
 void process_received(modular_structs_t *structs) {
   // Parse as many packets as possible
-  struct UARTDriver *uart = &structs->hardware_struct.comm->uart;
+  struct UARTDriver *uart = structs->hardware_struct.comm.uart;
   while (!try_receive_packet(uart)) {
     process_packet(structs);
   }
diff --git a/quad/src/quad_app/initialize_components.c b/quad/src/quad_app/initialize_components.c
index b39bf638ff782b148457e012759c667567beee0a..edccbbbf88d06f4d48bc7bd24053eeb8d8dd43dc 100644
--- a/quad/src/quad_app/initialize_components.c
+++ b/quad/src/quad_app/initialize_components.c
@@ -64,7 +64,7 @@ int init_structs(modular_structs_t *structs) {
   timer_init_globals(global_timer, axi_timer);
 
   // Initialize UART0
-  struct UARTDriver *uart = &structs->hardware_struct.uart;
+  struct UARTDriver *uart = &structs->hardware_struct.uart_0;
   if (uart->reset(uart)) {
     return -1;
   }
diff --git a/quad/src/quad_app/quad_app.c b/quad/src/quad_app/quad_app.c
index 106a3e0382ef7965c777dbfc4f96e1e7db493432..42b32b75caca729cc694884e502aa2f0f7f5f527 100644
--- a/quad/src/quad_app/quad_app.c
+++ b/quad/src/quad_app/quad_app.c
@@ -116,7 +116,7 @@ int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
 		if (structs.raw_sensor_struct.gam.error.consErrorCount > 10) {
 			kill_motors(&(structs.hardware_struct.motors));
 			char err_msg[] = "More than 10 IMU errors";
-			send_data(structs.hardware_struct.comm->uart, DEBUG_ID, 0, (u8*)err_msg, sizeof(err_msg));
+			send_data(structs.hardware_struct.comm.uart, DEBUG_ID, 0, (u8*)err_msg, sizeof(err_msg));
 			printLogging(&structs.hardware_struct.comm, &(structs.log_struct), &(structs.parameter_struct), &structs.raw_sensor_struct);
 			break;
 		}
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.c
index 412478dcd7fb24eec992f246cb481273ef7c2524..093b67ef72eb7cbbfdc2976c1f2b5832d89817b5 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.c
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.c
@@ -2,7 +2,17 @@
 
 struct UARTDriver create_zybo_uart(int devId) {
   struct UARTDriver uart;
-  uart.state = NULL;
+  uart.state = malloc(sizeof(struct ZyboUARTState));
+  struct ZyboUARTState *state = uart.state;
+  state->inst = malloc(sizeof(XUartPs));
+
+  // We are cheating. We put the Queue on the callback field,
+  // because that was the only way we could get the Queue
+  // into the interupt handler. And the callback field was a void
+  // pointer so we can do whatever we want with it...
+  state->inst->CallBackRef = queue_malloc(MAX_UART_BUFFER_SIZE);
+  state->devId = devId;
+
   uart.reset = zybo_uart_reset;
   uart.write = zybo_uart_write;
   uart.read = zybo_uart_read;
@@ -41,7 +51,10 @@ struct RCReceiverDriver create_zybo_rc_receiver() {
 
 struct I2CDriver create_zybo_i2c(int busId) {
   struct I2CDriver i2c;
-  i2c.state = NULL;
+  i2c.state = malloc(sizeof(struct ZyboI2CState));
+  struct ZyboI2CState *state = i2c.state;
+  state->inst = malloc(sizeof(XIicPs));
+  state->busId = busId;
   i2c.reset = zybo_i2c_reset;
   i2c.write = zybo_i2c_write;
   i2c.read = zybo_i2c_read;
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.h b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.h
index 2f577ee3f7f6bdb60211d20d4c7a78c6c0381a5a..f47c34b058ff274b624c2283efaedfb6876d5740 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.h
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.h
@@ -24,6 +24,19 @@
 #define PX4FLOW_DEVICE_ADDR			0x42
 #define PX4FLOW_QUAL_MIN			(100)
 
+#define MAX_UART_BUFFER_SIZE 2048
+
+struct ZyboI2CState {
+  XIicPs *inst;
+  int busId;
+};
+
+struct ZyboUARTState {
+  struct Queue *queue;
+  XUartPs *inst;
+  int devId;
+};
+
 int zybo_uart_reset(struct UARTDriver *self);
 int zybo_uart_write(struct UARTDriver *self, unsigned char c);
 int zybo_uart_read(struct UARTDriver *self, unsigned char *c);
@@ -68,7 +81,12 @@ int zybo_lidar_read(struct LidarDriver *self, struct lidar *lidar);
 int zybo_optical_flow_reset(struct OpticalFlowDriver *self, struct px4flow *of);
 int zybo_optical_flow_read(struct OpticalFlowDriver *self, struct px4flow *of);
 
-struct UARTDriver create_zybo_uart();
+int zybo_gps_reset(struct GPSDriver *self, gps_t *gps);
+int zybo_gps_read(struct GPSDriver *self, gps_t *gps);
+
+struct UARTDriver create_zybo_uart(int devId);
+struct CommDriver create_zybo_comm(struct UARTDriver *uart);
+struct GPSDriver create_zybo_gps(struct UARTDriver *uart);
 struct MotorDriver create_zybo_motors();
 struct RCReceiverDriver create_zybo_rc_receiver();
 struct I2CDriver create_zybo_i2c(int busId);
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_gps.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_gps.c
new file mode 100644
index 0000000000000000000000000000000000000000..ff495afdbc2a597e3e2ca9ba89343cefeee6ed09
--- /dev/null
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_gps.c
@@ -0,0 +1,12 @@
+#include "hw_iface.h"
+#include "hw_impl_zybo.h"
+
+int zybo_gps_reset(struct GPSDriver *self, gps_t *gps) {
+  // TODO
+  return -1;
+}
+
+int zybo_gps_read(struct GPSDriver *self, gps_t *gps) {
+  // TODO
+  return -1;
+}
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_i2c.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_i2c.c
index fd436bf808888ec33d0e0cc27102fa94179195a8..34ea395e787bbdd8b5307718b3cfa49b8c088a9d 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_i2c.c
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_i2c.c
@@ -42,18 +42,10 @@ int XIicPs_MasterRecvPolled_ours(XIicPs *InstancePtr, u8 *MsgPtr,
 				int ByteCount, u16 SlaveAddr);
 int XIicPs_SetupMaster(XIicPs *InstancePtr, int Role);
 
-struct ZyboI2CState {
-  XIcPs *inst;
-  int busId;
-};
-
 int zybo_i2c_reset(struct I2CDriver *self) {
-  if (self->state == NULL) {
-    self->state = malloc(sizeof(struct ZyboI2CState));
-    self->state->inst = malloc(sizeof(XIcPs));
-  }
-  int i2cID = self->state.busId;
-  XIicPs *inst = self->state->inst;
+  struct ZyboI2CState *state = self->state;
+  int i2cID = state->busId;
+  XIicPs *inst = state->inst;
 
   //Make sure CPU_1x clk is enabled fostatusr I2C controller
   u16 *aper_ctrl = (u16 *) IO_CLK_CONTROL_REG_ADDR;
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_tests.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_tests.c
index 9c02bdb6338734c41c8afe352682366739410f86..bf7310ea5173078673971742da1b68a6f6d3dc56 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_tests.c
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_tests.c
@@ -280,7 +280,7 @@ int test_zybo_global_timer() {
 }
 
 /**
- * Test for the UARTDriver.
+ * Communication tests using the UARTDriver.
  *
  * Instructions:
  * 1) Connect Zybo board to computer by USB cable.
@@ -296,15 +296,16 @@ int test_zybo_global_timer() {
  *    - Observe test results on terminal
  *    - You might be able to see LED MIO7 blink when it receives bytes
  */
-int test_zybo_uart() {
-  struct UARTDriver uart = create_zybo_uart();
+int test_zybo_uart_comm() {
+  struct UARTDriver uart = create_zybo_uart(0);
+  struct CommDriver comm = create_zybo_comm(&uart);
   struct LEDDriver led = create_zybo_mio7_led();
   uart.reset(&uart);
   led.reset(&led);
 
   unsigned char c;
   while (1) {
-    if (uart.read(&uart, &c)) {
+    if (comm.uart->read(comm.uart, &c)) {
       // read failed
       led.turn_off(&led);
     } else {
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_uart.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_uart.c
index 0baaab96cd8ccea7597396b321ee4060336f6934..dd9a2ca17267ab3e4d9b583e8809b312d859fc84 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_uart.c
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_uart.c
@@ -1,35 +1,21 @@
 #include "hw_impl_zybo.h"
 
 #define BAUD_RATE 921600
-#define MAX_UART_BUFFER_SIZE 2048
+
 
 int XUartPs_SetBaudRate_ours(XUartPs *InstancePtr, u32 BaudRate);
 void uart_interrupt_handler(XUartPs *InstancePtr);
 int SetupInterruptSystem(XUartPs *UartInstancePtr, u16 UartIntrId, Xil_ExceptionHandler handler);
 void uart_interrupt_handler(XUartPs *InstancePtr);
 
-struct UARTState {
-  struct Queue *queue;
-  XUartPs *inst;
-};
-
 int zybo_uart_reset(struct UARTDriver *self) {
-  // Instantiate the Driver state
-  if (self->state == NULL) {
-    self->state = malloc(sizeof(struct UARTState));
-    if (self->state == NULL) return -1;
-    self->state->inst = malloc(sizeof(XUartPs));
-    if (self->state->inst == NULL) return -1;
-
-    // We are cheating. We put the Queue on the callback field,
-    // because that was the only way we could get the Queue
-    // into this interupt handler. And the callback field was a void
-    // pointer so we can do whatever we want with it...
-    self->state->inst->CallBackRef = queue_malloc(MAX_UART_BUFFER_SIZE);
-    if (self->state->inst->CallBackRef == NULL) return -1;
-  }
+  // Ensure all required memory is allocated
+  struct ZyboUARTState *state = self->state;
+  if (state == NULL) return -1;
+  if (state->inst == NULL) return -1;
+  if (state->inst->CallBackRef == NULL) return -1;
 
-  XUartPs *inst = self->state->inst;
+  XUartPs *inst = state->inst;
 
   // Configure XUartPs instance
   XUartPs_Config* config = XUartPs_LookupConfig(XPAR_PS7_UART_0_DEVICE_ID);
@@ -81,8 +67,9 @@ int zybo_uart_write(struct UARTDriver *self, unsigned char c) {\
 }
 
 int zybo_uart_read(struct UARTDriver *self, unsigned char *c) {
+  struct ZyboUARTState *state = self->state;
   // We put the queue on the void pointer callback reference
-  struct Queue *queue = InstancePtr->CallBackRef;
+  struct Queue *queue = state->inst->CallBackRef;
   if (queue_remove(queue, c)) return -1;
   else return 0;
 }