wip: Separate Xilinx code into hardware interfaces

quad/.gitignore

 zybo_fsbl_bsp/
 zybo_fsbl/
 test.log
+inc/
+obj/
+TAGS
\ No newline at end of file

quad/lib/queue/.gitignore

+test
\ No newline at end of file

quad/lib/queue/Makefile

+OBJ = ../../obj
+INC = ../../inc
+
+queue: queue.h queue.c
+	[ -d $(OBJ) ] ||  mkdir -p $(OBJ)
+	[ -d $(INC) ] || mkdir -p $(INC)
+	gcc -c -o $(OBJ)/libqueue.a queue.c
+	cp queue.h $(INC)/
+
+.PHONY : test
+test: queue test_queue.c
+	gcc -o test test_queue.c -I$(INC) -L$(OBJ) -lqueue -ltest
+	./test

quad/lib/queue/queue.c

+#include "queue.h"
+
+struct Queue queue_create(unsigned int max_size) {
+  struct Queue q;
+  q.buff = malloc(sizeof(char) * (max_size + 1));
+  q.max_size = max_size + 1;
+  q.start = max_size;
+  q.end = 0;
+  return q;
+}
+
+int queue_init(struct Queue *q, unsigned int max_size) {
+  q->buff = malloc(sizeof(unsigned char) * (max_size + 1));
+  if (q->buff == NULL) return -1;
+  q->max_size = max_size + 1; // required for start/end pointers
+  q->start = max_size;
+  q->end = 0;
+  return 0;
+}
+
+void queue_destroy(struct Queue *q) {
+  free(q->buff);
+}
+
+int queue_add(volatile struct Queue *q, unsigned char c) {
+  if (queue_full(q)) {
+    return -1;
+  }
+  q->buff[q->end] = c;
+  q->end += 1;
+  q->end %= q->max_size;
+  return 0;
+}
+
+int queue_remove(volatile struct Queue *q, unsigned char *c) {
+  if (queue_empty(q)) {
+    return -1;
+  }
+  q->start += 1;
+  q->start %= q->max_size;
+  *c = q->buff[q->start];
+  return 0;
+}
+
+int queue_size(volatile struct Queue *q) {
+  return (q->max_size + q->end - q->start - 1) % q->max_size;
+}
+
+int queue_full(volatile struct Queue *q) {
+  return q->start == q->end;;
+}
+
+int queue_empty(volatile struct Queue *q) {
+  return queue_size(q) == 0;
+}
+
+void queue_print(struct Queue *q) {
+  int i;
+  printf("buffer contents:\n");
+  for (i = 0; i < q->max_size; i += 1) {
+    char c = (char) q->buff[i];
+    if (c == 0) c = '-';
+    printf("%c", c);;
+    if (i == q->start) printf("  <- start");
+    if (i == q->end) printf("  <- end");
+    printf("\n");
+  }
+}

quad/lib/queue/queue.h

+#ifndef QUEUE_H
+#define QUEUE_H
+
+#include <stdlib.h>
+#include <stdio.h>
+
+struct Queue {
+  unsigned char *buff;
+  unsigned int max_size;
+  unsigned int start;
+  unsigned int end;
+};
+
+
+struct Queue queue_create(unsigned int max_size);
+int queue_init(struct Queue *q, unsigned int max_size);
+void queue_destory(struct Queue *q);
+int queue_add(volatile struct Queue *q, unsigned char c);
+int queue_remove(volatile struct Queue *q, unsigned char *c);
+int queue_size(volatile struct Queue *q);
+int queue_full(volatile struct Queue *q);
+int queue_empty(volatile struct Queue *q);
+void queue_print(struct Queue *q);
+
+#endif

quad/lib/queue/test_queue.c

+#include "test.h"
+#include "queue.h"
+
+int test_add() {
+  puts("---");
+  puts("test_add");
+  int failed = 0;
+  struct Queue *q = malloc(sizeof(struct Queue));
+  queue_init(q, 5);
+  puts("initialized");
+  unsigned char c;
+  failed = failed || queue_add(q, 'a');
+  return failed;
+}
+
+int test_size() {
+  puts("---");
+  puts("test_size");
+  int failed = 0;
+  struct Queue *q = malloc(sizeof(struct Queue));
+  queue_init(q, 4);
+  unsigned char c;
+  failed = failed || queue_size(q) != 0;
+  printf("size: %d\n", queue_size(q));
+  queue_print(q);
+
+  queue_add(q, 'a');
+
+  printf("size: %d\n", queue_size(q));
+  queue_print(q);
+  failed = failed || queue_size(q) != 1;
+
+  queue_add(q, 'b');
+
+  printf("size: %d\n", queue_size(q));
+  queue_print(q);
+  failed = failed || queue_size(q) != 2;
+
+  queue_add(q, 'c');
+
+  printf("size: %d\n", queue_size(q));
+  queue_print(q);
+  failed = failed || queue_size(q) != 3;
+
+  queue_add(q, 'd');
+
+  printf("size: %d\n", queue_size(q));
+  queue_print(q);
+  failed = failed || queue_size(q) != 4;
+
+  queue_remove(q, &c);
+  queue_remove(q, &c);
+
+  printf("size: %d\n", queue_size(q));
+  queue_print(q);
+  failed = failed || queue_size(q) != 2;
+
+  queue_add(q, 'e');
+
+  printf("size: %d\n", queue_size(q));
+  queue_print(q);
+  failed = failed || queue_size(q) != 3;
+
+  return failed;
+}
+
+int test_full_when_full() {
+  puts("---");
+  puts("test_full_when_full");
+  int failed = 0;
+  struct Queue *q = malloc(sizeof(struct Queue));
+  queue_init(q, 4);
+  unsigned char c;
+  queue_add(q, 'a');
+  queue_add(q, 'b');
+  queue_add(q, 'c');
+  queue_add(q, 'd');
+  failed = failed || !queue_full(q);
+  return failed;
+}
+
+int test_add_when_full() {
+  puts("---");
+  puts("test_add_when_full");
+  int failed = 0;
+  struct Queue *q = malloc(sizeof(struct Queue));
+  queue_init(q, 4);
+  unsigned char c;
+  queue_add(q, 'a');
+  queue_add(q, 'b');
+  queue_add(q, 'c');
+  queue_add(q, 'd');
+  queue_print(q);
+  failed = failed || !queue_add(q, 'x');
+  queue_remove(q, &c);
+  queue_print(q);
+  failed = failed || !(c == 'a');
+  queue_remove(q, &c);
+  queue_print(q);
+  failed = failed || !(c == 'b');
+  queue_remove(q, &c);
+  queue_print(q);
+  failed = failed || !(c == 'c');
+  queue_remove(q, &c);
+  queue_print(q);
+  failed = failed || !(c == 'd');
+  return failed;
+}
+
+int test_add_over_edge() {
+  int failed = 0;
+  struct Queue *q = malloc(sizeof(struct Queue));
+  queue_init(q, 4);
+  unsigned char c;
+  queue_add(q, 'x');
+  queue_add(q, 'x');
+  queue_remove(q, &c);
+  queue_remove(q, &c);
+  queue_add(q, 'a');
+  queue_add(q, 'b');
+  queue_add(q, 'c');
+  queue_add(q, 'd');
+  queue_print(q);
+  failed = failed || !queue_add(q, 'x');
+  queue_remove(q, &c);
+  failed = failed || !(c == 'a');
+  queue_remove(q, &c);
+  failed = failed || !(c == 'b');
+  queue_remove(q, &c);
+  failed = failed || !(c == 'c');
+  queue_remove(q, &c);
+  failed = failed || !(c == 'd');
+  return failed;
+}
+
+int main() {
+  test(test_add, "test_add");
+  test(test_size, "test_size");
+  test(test_full_when_full, "test_full_when_full");
+  test(test_add_when_full, "test_add_when_full");
+  test(test_add_over_edge, "test_add_over_edge");
+  return test_summary();
+}

quad/lib/test/Makefile

-test.o: test.c
-	gcc -c test.c
+OBJ = ../../obj
+INC = ../../inc
 
-example: example.c test.c test.h
-	gcc -g -o example example.c test.c test.h
+test: test.c test.h
+	[ -d $(OBJ) ] || mkdir -p $(OBJ)
+	[ -d $(INC) ] || mkdir -p $(INC)
+	gcc -c -o $(OBJ)/libtest.a test.c
+	cp test.h $(INC)
+
+example: test_lib example.c
+	gcc -g -o example example.c -I$(INC) -L$(OBJ) -ltest
 
 .PHONY: clean
 clean:

quad/sw/.gitignore

@@ -2,4 +2,4 @@ SDK.log
 test.log
 .metadata/
 system_bsp/
-
+TAGS
\ No newline at end of file

quad/sw/imu_logger/src/iic_mpu9150_utils.h

@@ -15,9 +15,8 @@
 #define IIC_MPU9150_UTILS_H
 
 
-#include "xbasic_types.h"
-#include "xiicps.h"
 #include "type_def.h"
+#include "hardware/hw_iface.h"
 
 // System configuration registers
 // (Please see Appendix B: System Level Control Registers in the Zybo TRM)
@@ -97,6 +96,8 @@
 #define ACCEL_Y_BIAS	0.009f
 #define ACCEL_Z_BIAS	0.087f
 
+void iic_set_global(struct I2CDriver *given_i2c);
+
 // Initialize hardware; Call this FIRST before calling any other functions
 int initI2C0();
 
@@ -134,14 +135,14 @@ void stop_mpu9150();
 
 
 // Write a byte of data at the given register address on the MPU
-void iic0_write(Xuint16 reg_addr, Xuint8 data);
+void iic0_write(u16 reg_addr, u8 data);
 
 // Read a single byte at a given register address on the MPU
-Xuint8 iic0_read(Xuint16 reg_addr);
+u8 iic0_read(u16 reg_addr);
 
 // Read multiple bytes consecutively at a starting register address
 // places the resulting bytes in rv
-int iic0_read_bytes(Xuint8* rv, Xuint16 reg_addr, int bytes);
+int iic0_read_bytes(u8* rv, u16 reg_addr, int bytes);
 
 // Helper function to initialize I2C0 controller on the Zybo board
 // Called by init_iic0

quad/sw/modular_quad_pid/src/callbacks.c

 #include "communication.h"
 #include "commands.h"
 #include "type_def.h"
-#include "uart.h"
 #include "computation_graph.h"
+#include "util.h"
 
 /*
  * Static variables used to keep track of packet counts
@@ -16,26 +16,26 @@ static size_t total_payload_received = 0;
 /**
   * Currently does nothing.
   */
-int cb_debug(modular_structs_t *structs)
+int cb_debug(modular_structs_t *structs, metadata_t *meta, u8 *data, u16 length)
 {
 	char buf[255];
 
 	// Get the node ID, parameter ID, parameter value
-	u8 node_id = uart_buff_data_get_u8(0);
+	u8 node_id = data[0];
 	struct computation_graph* graph = structs->parameter_struct.graph;
 	float param_val = graph_get_output(graph, node_id, 0);
 
-	int length = snprintf(buf, sizeof buf, "%f", param_val);
-	send_data(DEBUG_ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
+	int len = snprintf(buf, sizeof buf, "%f", param_val);
+	send_data(&structs->hardware_struct.uart, DEBUG_ID, 0, buf, len >= sizeof(buf) ? 255 : length + 1);
 	return 0;
 }
 
 /**
   * counts the number of packet logs.
   */
-int cb_packetlog(modular_structs_t* structs) {
+int cb_packetlog(modular_structs_t* structs, metadata_t *meta, u8 *data, u16 length) {
 	n_msg_received += 1;
-	total_payload_received += uart_buff_data_length();
+	total_payload_received += length;
 	return 0;
 }
 
@@ -43,38 +43,38 @@ int cb_packetlog(modular_structs_t* structs) {
   * Handles a get packet logs request and sends a response
   * with the packet log data.
   */
-int cb_getpacketlogs(modular_structs_t* structs) {
+int cb_getpacketlogs(modular_structs_t* structs, metadata_t *meta, u8 *data, u16 length) {
 	char buf[255];
 
 	// Message logging number of messages received and size of payload received
-	int length = snprintf(buf, sizeof buf, "%d,%d", n_msg_received, total_payload_received);
+	int len = snprintf(buf, sizeof buf, "%d,%d", n_msg_received, total_payload_received);
 
-	send_data(LOG_ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
+	send_data(&structs->hardware_struct.uart, LOG_ID, 0, buf, len >= sizeof(buf) ? 255 : len + 1);
 	return 0;
 }
 
 /*
  * Handles receiving new location updates.
  */
-int cb_update(modular_structs_t *structs)
+int cb_update(modular_structs_t *structs, metadata_t *meta, u8 *data, u16 length)
 {
 	//processUpdate(packet, &(structs->raw_sensor_struct.currentQuadPosition));
 
 	quadPosition_t* currentQuadPosition = &(structs->raw_sensor_struct.currentQuadPosition);
 	// Packet must come as [NEARPY], 4 bytes each
-	int packetId = uart_buff_data_get_u32(0);
+	int packetId = build_int(data + 0);
 //	printf("Packet ID: %d\n", packetId);
-	float y_pos = uart_buff_data_get_float(4);
+	float y_pos = build_float(data + 4);
 //	printf("y_pos: %f\n", y_pos);
-	float x_pos = uart_buff_data_get_float(8);
+	float x_pos = build_float(data + 8);
 //	printf("x_pos: %f\n", x_pos);
-	float alt_pos = uart_buff_data_get_float(12);
+	float alt_pos = build_float(data + 12);
 //	printf("alt_pos: %f\n", alt_pos);
-	float roll = uart_buff_data_get_float(16);
+	float roll = build_float(data + 16);
 //	printf("roll: %f\n", roll);
-	float pitch = uart_buff_data_get_float(20);
+	float pitch = build_float(data + 20);
 //	printf("pitch: %f\n", pitch);
-	float yaw = uart_buff_data_get_float(24);
+	float yaw = build_float(data + 24);
 //	printf("yaw: %f\n", yaw);
 
 	currentQuadPosition->packetId = packetId;
@@ -94,7 +94,7 @@ int cb_update(modular_structs_t *structs)
 /**
   * This is called on the ground station to begin sending VRPN to the quad.
   */
-int cb_beginupdate(modular_structs_t *structs) {
+int cb_beginupdate(modular_structs_t *structs, metadata_t *meta, u8 *data, u16 length) {
 	structs->user_input_struct.receivedBeginUpdate = 1;
 	return 0;
 }
@@ -117,10 +117,10 @@ int cb_beginupdate(modular_structs_t *structs) {
   * 
   * Does not send anything in response.
   */
-int cb_setparam(modular_structs_t *structs)
+int cb_setparam(modular_structs_t *structs, metadata_t *meta, u8 *data, u16 length)
 {
 	// Get some of the meta data
-	u16 data_len = uart_buff_data_length();
+	u16 data_len = length;
 	// Check if the data length is correct
 	if (data_len != 6)
 	{
@@ -129,9 +129,9 @@ int cb_setparam(modular_structs_t *structs)
 	struct computation_graph* graph = structs->parameter_struct.graph;
 
 	// Get the node ID, parameter ID, parameter value
-	u8 node_id = uart_buff_data_get_u8(0);
-	u8 param_id = uart_buff_data_get_u8(1);
-	float param_val = uart_buff_data_get_float(2);
+	u8 node_id = data[0];
+	u8 param_id = data[1];
+	float param_val = build_float(data + 2);
 	// Set the value for that parameter on that node
 	graph_set_param_val(graph, node_id, param_id, param_val);
 
@@ -162,11 +162,11 @@ int cb_setparam(modular_structs_t *structs)
   * |       bytes ||      1      |      1      |      4      |
   * |--------------------------------------------------------|
   */
-int cb_getparam(modular_structs_t* structs)
+int cb_getparam(modular_structs_t* structs, metadata_t *meta,  u8 *data, u16 length)
 {
 	// Get some of the meta data
-	u16 data_len = uart_buff_data_length();
-	u16 msg_id = uart_buff_get_u16(3);
+	u16 data_len = length;
+	u16 msg_id = meta->msg_id;
 	// Check if the data length is correct
 	if (data_len != 2)
 	{
@@ -174,8 +174,8 @@ int cb_getparam(modular_structs_t* structs)
 	}
 
 	// Get the controller ID, parameter ID
-	u8 node_id = uart_buff_data_get_u8(0);
-	u8 param_id = uart_buff_data_get_u8(1);
+	u8 node_id = data[0];
+	u8 param_id = data[1];
 	struct computation_graph* graph = structs->parameter_struct.graph;
 	float param_val = graph_get_param_val(graph, node_id, param_id);
 
@@ -190,7 +190,7 @@ int cb_getparam(modular_structs_t* structs)
 	memcpy(&resp_data[2], &param_val, sizeof(param_val));
 
 	// Send the response
-	send_data(RESPPARAM_ID, msg_id, resp_data, sizeof(resp_data));
+	send_data(&structs->hardware_struct.uart, RESPPARAM_ID, msg_id, resp_data, sizeof(resp_data));
 
 	return 0;
 }

quad/sw/modular_quad_pid/src/callbacks.h

@@ -2,10 +2,10 @@
 #define __callbacks_h
 
 /* Grab some stupid stuff from legacy code */
-
 struct modular_structs;
+struct metadata;
 
 /* Make commands.c happy */
-typedef int (command_cb)(struct modular_structs *structs);
+typedef int (command_cb)(struct modular_structs *, struct metadata *, unsigned char *, unsigned short);
 
 #endif

quad/sw/modular_quad_pid/src/communication.c

 #include "communication.h"
-#include "uart_buff.h"
+#include "hardware/hw_iface.h"
 
 #define INTC		XScuGic
 #define COMM_UART_DEVICE_ID		XPAR_PS7_UART_0_DEVICE_ID
@@ -12,6 +12,7 @@
 // plus 128 for a little buffer
 // (bit/s) * (seconds) / (10 bits / byte for UART)
 #define MAX_PACKET_SIZE 256
+#define PACKET_HEADER_SIZE 7
 #define UART_BUF_SIZE (((BAUD_RATE * (DESIRED_USEC_PER_LOOP / 1000) / 1000) / 10) + MAX_PACKET_SIZE + 128)
 
 //#define INTERRUPT_BENCHMARK
@@ -19,154 +20,75 @@
 // Declaration of interrupt handler
 void Handler(void *CallBackRef, u32 Event, unsigned int EventData);
 
-// Pointer to the UART driver instance
-static XUartPs* uartInstPtr;
-
-
-int initUartComms() {
-	uartInstPtr = uart0_init(COMM_UART_DEVICE_ID, BAUD_RATE);
-	// Initialize UART0 (Bluetooth/WiFi)
-	if(!uartInstPtr) {
-		return -1;
-	}
-
-	uart0_clearFIFOs();
-
-	if (uart0_int_init(COMM_UART_INT_IRQ_ID, (Xil_ExceptionHandler) uart_interrupt_handler) != XST_SUCCESS) {
-		return -1;
-	}
-
-
-	/*
-	 * Setup the handlers for the UART that will be called from the
-	 * interrupt context when data has been sent and received, specify
-	 * a pointer to the UART driver instance as the callback reference
-	 * so the handlers are able to access the instance data
-	 */
-	//XUartPs_SetHandler(uartInstPtr, (XUartPs_Handler)Handler, uartInstPtr);
-
-	u32 IntrMask = XUARTPS_IXR_RXFULL | XUARTPS_IXR_RXOVR | XUARTPS_IXR_TOUT;
-
-	XUartPs_SetInterruptMask(uartInstPtr, IntrMask);
-
-
-	/*
-	 * Set the receiver timeout. If it is not set, and the last few bytes
-	 * of data do not trigger the over-water or full interrupt, the bytes
-	 * will not be received. By default it is disabled.
-	 * Timeout duration = RecvTimeout x 4 x Bit Period. 0 disables the
-	 *		timeout function.
-	 *
-	 * The setting of 8 will timeout after 8 x 4 = 32 character times.
-	 * Increase the time out value if baud rate is high, decrease it if
-	 * baud rate is low.
-	 */
-	XUartPs_SetRecvTimeout(uartInstPtr, 8);
-
-	// Second argument is the number of bytes to trigger an interrupt at
-	XUartPs_SetFifoThreshold(uartInstPtr, 48);
-
-	return 0;
+static u8 packet[MAX_PACKET_SIZE];
+static int bytes_recv = 0;
+
+int try_receive_packet(struct UARTDriver *uart) {
+
+  // Find start of packet
+  int attempts = 0;
+  while (bytes_recv == 0) {
+    if (attempts > MAX_PACKET_SIZE) return -1; // keep trying later
+    if (uart->read(uart, &packet[bytes_recv], 1)) return -1; // ran out
+    if (packet[bytes_recv] == 0xBE) bytes_recv += 1;
+    attempts += 1;
+  }
+
+  // Find length bytes in packet
+  while (bytes_recv < PACKET_HEADER_SIZE) {
+    if (uart->read(uart, &packet[bytes_recv++], 1)) return -1; // ran out
+    bytes_recv += 1;
+  }
+
+  // Wait for rest of data plus checksum (+1)
+  int length = (packet[6] << 8 | packet[5]) + PACKET_HEADER_SIZE + 1;
+  while (bytes_recv < length) {
+    if (uart->read(uart, &packet[bytes_recv++], 1)) return -1; // ran out
+    bytes_recv += 1;
+  }
+
+  // Fully recieved a packet
+  return 0;
 }
 
 int process_packet(modular_structs_t *structs) {
-	metadata_t meta_data;
-	// parse metadata
-	meta_data.begin_char = uart_buff_get_u8(0);
-	meta_data.msg_type = uart_buff_get_u16(1);
-	meta_data.msg_id = uart_buff_get_u16(3);
-	meta_data.data_len = uart_buff_get_u16(5);
-	unsigned char packet_checksum = uart_buff_get_u8(7+meta_data.data_len);
-	//unsigned char* packet_data = packet + sizeof(metadata_t);
-
-	// Compute checksum
-	int i;
-	unsigned char calculated_checksum = 0;
-	for(i = 0; i < meta_data.data_len + 7; i++){
-		calculated_checksum ^= uart_buff_get_u8(i);
-	}
-	// Discard if checksum didn't match
-	if(packet_checksum != calculated_checksum) {
-		uart_buff_consume_packet();
-		return -1;
-	}
-
-	// Call appropriate function for packet
-	(* (MessageTypes[meta_data.msg_type].functionPtr))(structs);
-
-	uart_buff_consume_packet();
-
-	return 0;
-
-}
-
-/*
- * Temporarily disables interrupts and returns the interrupt state, for restoring them
- */
-u32 disable_interrupts() {
-	u32 ImrRegister;
-	ImrRegister = XUartPs_ReadReg(uartInstPtr->Config.BaseAddress, XUARTPS_IMR_OFFSET);
-	XUartPs_WriteReg(uartInstPtr->Config.BaseAddress, XUARTPS_IDR_OFFSET, XUARTPS_IXR_MASK);
-	return ImrRegister;
-}
-
-/*
- * Restores the interrupt state, saved from disable_interrupts
- */
-void restore_interrupts(u32 intr_state) {
-	XUartPs_WriteReg(uartInstPtr->Config.BaseAddress, XUARTPS_IER_OFFSET, intr_state);
+  metadata_t meta;
+  // parse metadata
+  meta.begin_char = packet[0];
+  meta.msg_type = packet[2] << 8 | packet[1];
+  meta.msg_id = packet[4] << 8 | packet[3];
+  meta.data_len = packet[6] << 8 | packet[5];
+  unsigned char packet_checksum = packet[7+meta.data_len];
+
+  // Compute checksum
+  int i;
+  unsigned char calculated_checksum = 0;
+  for(i = 0; i < meta.data_len + 7; i++){
+    calculated_checksum ^= packet[i];
+  }
+  // Discard if checksum didn't match
+  if(packet_checksum != calculated_checksum) {
+    return -1;
+  }
+
+  // Call appropriate function for packet
+  (* (MessageTypes[meta.msg_type].functionPtr))(structs,&meta,packet[PACKET_HEADER_SIZE], meta.data_len);
+
+  // Done processing packets, reset
+  bytes_recv = 0;
+
+  return 0;
 }
 
 void process_received(modular_structs_t *structs) {
-	// Parse as many packets as possible
-	while (uart_buff_packet_ready()) {
-		process_packet(structs);
-	}
-}
-
-void uart_interrupt_handler(XUartPs *InstancePtr) {
-	u32 IsrStatus;
-
-	/*
-	 * Read the interrupt ID register to determine which
-	 * interrupt is active
-	 */
-	IsrStatus = XUartPs_ReadReg(InstancePtr->Config.BaseAddress,
-				   XUARTPS_IMR_OFFSET);
-
-	IsrStatus &= XUartPs_ReadReg(InstancePtr->Config.BaseAddress,
-				   XUARTPS_ISR_OFFSET);
-
-	/*
-	* Read the Channel Status Register to determine if there is any data in
-	 * the RX FIFO
-	 */
-
-	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_add_u8(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);
+  // Parse as many packets as possible
+  struct UARTDriver *uart = &structs->hardware_struct.uart;
+  while (try_receive_packet(uart)) {
+    process_packet(structs);
+  }
 }
 
-int send_data(u16 type_id, u16 msg_id, char* data, size_t size) {
+int send_data(struct UARTDriver *uart, u16 type_id, u16 msg_id, char* data, size_t size) {
 	//----------------------------------------------------------------------------------------------
 	//	   index||	   0	|	  1	   |	  2		 |	3 & 4 |		 5 & 6		 |	7+	|	end	   |
 	//---------------------------------------------------------------------------------------------|
@@ -175,7 +97,7 @@ int send_data(u16 type_id, u16 msg_id, char* data, size_t size) {
 	//	   bytes||	   1	|	       2             |   2    |        2         | var	|	 1	   |
 	//----------------------------------------------------------------------------------------------
 
-	char formattedHeader[7];
+	unsigned char formattedHeader[PACKET_HEADER_SIZE];
 
 	// Begin Char:
 	formattedHeader[0] = BEGIN_CHAR;
@@ -189,22 +111,20 @@ int send_data(u16 type_id, u16 msg_id, char* data, size_t size) {
 	formattedHeader[5] = size & 0x000000ff;
 	formattedHeader[6] = (size >> 8) & 0x000000ff;
 
-	// Compute checksum while sending
+	// Compute checksum
 	unsigned char packet_checksum = 0;
-
 	int i;
-	for(i = 0; i < 7; i++) {
+	for(i = 0; i < PACKET_HEADER_SIZE; i++) {
 		packet_checksum ^= formattedHeader[i];
-		uart0_sendByte(formattedHeader[i]);
 	}
-	// Send data
 	for (i = 0; i < size; i++) {
 		packet_checksum ^= data[i];
-		uart0_sendByte(data[i]);
 	}
-	//uart0_sendBytes(data, size);
-	// Send checksum
-	uart0_sendByte(packet_checksum);
+
+	// Send header, data, and checksum
+	uart->write(uart, formattedHeader, PACKET_HEADER_SIZE);
+	uart->write(uart, (unsigned char *) data, size);
+	uart->write(uart, &packet_checksum, 1);
 
 	return 0;
 }

quad/sw/modular_quad_pid/src/communication.h

 #ifndef _COMMUNICATION_H
 #define _COMMUNICATION_H
 
-#include <xuartps.h>
-#include "xparameters.h"
-#include "xscugic.h"
-#include "xil_exception.h"
-
 #include "type_def.h"
-#include "uart.h"
 #include "mio7_led.h"
 #include "timer.h"
 #include "commands.h"
-#include "uart_buff.h"
+#include "hardware/hw_iface.h"
 
 int initUartComms();
 void process_received(modular_structs_t *structs);
-void uart_interrupt_handler(XUartPs *InstancePtr);
-int send_data(u16 type_id, u16 msg_id, char* data, size_t size);
+int send_data(struct UARTDriver *uart, u16 type_id, u16 msg_id, char* data, size_t size);
 
 #endif

quad/sw/modular_quad_pid/src/controllers.c

@@ -12,7 +12,6 @@
 #include "iic_utils.h"
 #include "quadposition.h"
 #include "util.h"
-#include "uart.h"
 #include "sleep.h"
 #include "stdio.h"
 #include <math.h>

quad/sw/modular_quad_pid/src/gam.h

 #ifndef _GAM_H
 #define _GAM_H
 
-#include "xbasic_types.h"
-
 //Gyro, accelerometer, and magnetometer data structure
 //Used for reading an instance of the sensor data
 typedef struct {

quad/sw/modular_quad_pid/src/hardware/hw_iface.h

+#ifndef HW_IFACE_H
+#define HW_IFACE_H
+
+struct I2CDriver {
+  void *state;
+  int (*reset)(struct I2CDriver *self);
+  int (*write)(struct I2CDriver *self,
+               unsigned short device_addr,
+               unsigned char *data,
+               unsigned int length);
+  int (*read)(struct I2CDriver *self,
+              unsigned short device_addr,
+              unsigned char *buff,
+              unsigned int length);
+};
+
+struct PWMOutputDriver {
+  void *state;
+  int (*reset)(struct PWMOutputDriver *self);
+  int (*write)(struct PWMOutputDriver *self, unsigned int channel, unsigned long pulse_width_us);
+};
+
+struct PWMInputDriver {
+  void *state;
+  int (*reset)(struct PWMInputDriver *self);
+  int (*read)(struct PWMInputDriver *self, unsigned int channel, unsigned long *pulse_width_us);
+};
+
+struct UARTDriver {
+  void *state;
+  int (*reset)(struct UARTDriver *self);
+  int (*write)(struct UARTDriver *self, unsigned char *data, unsigned int length);
+  int (*read)(struct UARTDriver *self, unsigned char *buff, unsigned int length);
+};
+
+struct TimerDriver {
+  void *state;
+  int (*reset)(struct TimerDriver *self);
+  int (*restart)(struct TimerDriver *self);
+  int (*read)(struct TimerDriver *self, unsigned long *us);
+};
+
+struct LEDDriver {
+  void *state;
+  int (*reset)(struct LEDDriver *self);
+  int (*turn_on)(struct LEDDriver *self);
+  int (*turn_off)(struct LEDDriver *self);
+};
+
+#endif

quad/sw/modular_quad_pid/src/hardware/hw_impl_zybo.h

+#ifndef HW_IMPL_ZYBO
+#define HW_IMPL_ZYBO
+
+#include "hw_iface.h"
+
+#include <sleep.h>
+#include <stdlib.h>
+#include <xtmrctr.h>
+#include <xgpiops.h>
+#include "xiicps.h"
+#include "xparameters.h"
+#include "xgpiops.h"
+#include "xil_types.h"
+#include "xscugic.h"
+#include "xtime_l.h"
+#include "xuartps.h"
+#include "queue.h"
+
+int zybo_uart_reset(struct UARTDriver *self);
+int zybo_uart_write(struct UARTDriver *self, unsigned char *data, unsigned int length);
+int zybo_uart_read(struct UARTDriver *self, unsigned char *buff, unsigned int length);
+
+int zybo_pwm_output_reset(struct PWMOutputDriver *self);
+int zybo_pwm_output_write(struct PWMOutputDriver *self, unsigned int channel, unsigned long pulse_width_us);
+
+int zybo_pwm_input_reset(struct PWMInputDriver *self);
+int zybo_pwm_input_read(struct PWMInputDriver *self, unsigned int channel, unsigned long *pulse_width_us);
+
+int zybo_i2c_reset(struct I2CDriver *self);
+int zybo_i2c_write(struct I2CDriver *self,
+                   unsigned short device_addr,
+                   unsigned char *data,
+                   unsigned int length);
+int zybo_i2c_read(struct I2CDriver *self,
+                  unsigned short device_addr,
+                  unsigned char *buff,
+                  unsigned int length);
+
+int zybo_global_timer_reset(struct TimerDriver *self);
+int zybo_global_timer_restart(struct TimerDriver *self);
+int zybo_global_timer_read(struct TimerDriver *self, unsigned long *us);
+
+int zybo_axi_timer_reset(struct TimerDriver *self);
+int zybo_axi_timer_restart(struct TimerDriver *self);
+int zybo_axi_timer_read(struct TimerDriver *self, unsigned long *us);
+
+int zybo_led_reset(struct LEDDriver *self);
+int zybo_led_turn_on(struct LEDDriver *self);
+int zybo_led_turn_off(struct LEDDriver *self);
+
+#endif

quad/sw/modular_quad_pid/src/hardware/hw_impl_zybo_axi_timer.c

+#include "hw_impl_zybo.h"
+
+// Apparently, this is number of counts per usec
+#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, unsigned long *us) {
+  // Returns the number of useconds
+  *us = XTmrCtr_GetValue(self->state, 0) / PL_CLK_CNTS_PER_USEC;
+  return 0;
+}

quad/sw/modular_quad_pid/src/hardware/hw_impl_zybo_global_timer.c

+#include "hw_impl_zybo.h"
+
+int zybo_global_timer_reset(struct TimerDriver *self) {
+  // Nothing to initialize
+  return 0;
+}
+
+int zybo_global_timer_restart(struct TimerDriver *self) {
+  XTime_SetTime(0);
+  return 0;
+}
+
+int zybo_global_timer_read(struct TimerDriver *self, unsigned long *us) {
+  XTime time;
+  XTime_GetTime(&time);
+  *us = (time * 1000000) / COUNTS_PER_SECOND; // (ticks)(1000000us/s)(s/ticks)
+  return 0;
+}

quad/sw/modular_quad_pid/src/hardware/hw_impl_zybo_i2c.c

+#include "hw_impl_zybo.h"
+
+#define IO_CLK_CONTROL_REG_ADDR	(0xF800012C)
+
+int XIicPs_MasterSendPolled_ours(XIicPs *InstancePtr, u8 *MsgPtr,
+				 int ByteCount, u16 SlaveAddr);
+int XIicPs_SetupMaster(XIicPs *InstancePtr, int Role);
+
+int zybo_i2c_reset(struct I2CDriver *self) {
+  if (self->state == NULL) {
+    self->state = malloc(sizeof(XIicPs));
+  }
+  XIicPs *inst = self->state;
+
+  //Make sure CPU_1x clk is enabled for I2C controller
+  u16 *aper_ctrl = (u16 *) IO_CLK_CONTROL_REG_ADDR;
+
+  if (*aper_ctrl & 0x00040000){
+    xil_printf("CPU_1x is set to I2C0\r\n");
+  }
+
+  else {
+    xil_printf("CPU_1x is not set to I2C0..Setting now\r\n");
+    *aper_ctrl |= 0x00040000;
+  }
+
+  // Look up
+  XIicPs_Config *i2c_config = XIicPs_LookupConfig(XPAR_PS7_I2C_0_DEVICE_ID);
+  XStatus status = XIicPs_CfgInitialize(inst, i2c_config, i2c_config->BaseAddress);
+
+  // Check if initialization was successful
+  if(status != XST_SUCCESS){
+    return -1;
+  }
+
+  // Reset the controller and set the clock to 400kHz
+  XIicPs_Reset(inst);
+  XIicPs_SetSClk(inst, 400000);
+
+  return 0;
+}
+
+int zybo_i2c_write(struct I2CDriver *self,
+                   unsigned short device_addr,
+                   unsigned char *data,
+                   unsigned int length) {
+  XIicPs *inst = self->state;
+  return XIicPs_MasterSendPolled_ours(inst, data, length, device_addr);
+}
+
+int zybo_i2c_read(struct I2CDriver *self,
+                  unsigned short device_addr,
+                  unsigned char *buff,
+                  unsigned int length) {
+  XIicPs *inst = self->state;
+  return XIicPs_MasterRecvPolled(inst, buff, length, device_addr);
+}
+
+/*****************************************************************************/
+/**
+* NOTE to MicroCART: This function is originally from the Xilinx library,
+* but we noticed that the original function didn't check for a NACK, which
+* would cause the original polling function to enter an infinite loop in the
+* event of a NACK. Notice that we have added that NACK check at the final
+* while loop of this function.
+*
+*
+* This function initiates a polled mode send in master mode.
+*
+* It sends data to the FIFO and waits for the slave to pick them up.
+* If slave fails to remove data from FIFO, the send fails with
+* time out.
+*
+* @param	InstancePtr is a pointer to the XIicPs instance.
+* @param	MsgPtr is the pointer to the send buffer.
+* @param	ByteCount is the number of bytes to be sent.
+* @param	SlaveAddr is the address of the slave we are sending to.
+*
+* @return
+*		- XST_SUCCESS if everything went well.
+*		- XST_FAILURE if timed out.
+*
+* @note		This send routine is for polled mode transfer only.
+*
+****************************************************************************/
+int XIicPs_MasterSendPolled_ours(XIicPs *InstancePtr, u8 *MsgPtr,
+		 int ByteCount, u16 SlaveAddr)
+{
+	u32 IntrStatusReg;
+	u32 StatusReg;
+	u32 BaseAddr;
+	u32 Intrs;
+
+	/*
+	 * Assert validates the input arguments.
+	 */
+	Xil_AssertNonvoid(InstancePtr != NULL);
+	Xil_AssertNonvoid(MsgPtr != NULL);
+	Xil_AssertNonvoid(InstancePtr->IsReady == XIL_COMPONENT_IS_READY);
+	Xil_AssertNonvoid(XIICPS_ADDR_MASK >= SlaveAddr);
+
+	BaseAddr = InstancePtr->Config.BaseAddress;
+	InstancePtr->SendBufferPtr = MsgPtr;
+	InstancePtr->SendByteCount = ByteCount;
+
+	XIicPs_SetupMaster(InstancePtr, SENDING_ROLE);
+
+	XIicPs_WriteReg(BaseAddr, XIICPS_ADDR_OFFSET, SlaveAddr);
+
+	/*
+	 * Intrs keeps all the error-related interrupts.
+	 */
+	Intrs = XIICPS_IXR_ARB_LOST_MASK | XIICPS_IXR_TX_OVR_MASK |
+			XIICPS_IXR_TO_MASK | XIICPS_IXR_NACK_MASK;
+
+	/*
+	 * Clear the interrupt status register before use it to monitor.
+	 */
+	IntrStatusReg = XIicPs_ReadReg(BaseAddr, XIICPS_ISR_OFFSET);
+	XIicPs_WriteReg(BaseAddr, XIICPS_ISR_OFFSET, IntrStatusReg);
+
+	/*
+	 * Transmit first FIFO full of data.
+	 */
+	TransmitFifoFill(InstancePtr);
+
+	IntrStatusReg = XIicPs_ReadReg(BaseAddr, XIICPS_ISR_OFFSET);
+
+	/*
+	 * Continue sending as long as there is more data and
+	 * there are no errors.
+	 */
+	while ((InstancePtr->SendByteCount > 0) &&
+		((IntrStatusReg & Intrs) == 0)) {
+		StatusReg = XIicPs_ReadReg(BaseAddr, XIICPS_SR_OFFSET);
+
+		/*
+		 * Wait until transmit FIFO is empty.
+		 */
+		if ((StatusReg & XIICPS_SR_TXDV_MASK) != 0) {
+			IntrStatusReg = XIicPs_ReadReg(BaseAddr,
+					XIICPS_ISR_OFFSET);
+			continue;
+		}
+
+		/*
+		 * Send more data out through transmit FIFO.
+		 */
+		TransmitFifoFill(InstancePtr);
+	}
+
+	/*
+	 * Check for completion of transfer.
+	 */
+	// NOTE for MicroCART: Corrected function. Original left for reference.
+//	while ((XIicPs_ReadReg(BaseAddr, XIICPS_ISR_OFFSET) &
+//		XIICPS_IXR_COMP_MASK) != XIICPS_IXR_COMP_MASK);
+	while (!(IntrStatusReg & (Intrs | XIICPS_IXR_COMP_MASK))) {
+		IntrStatusReg = XIicPs_ReadReg(BaseAddr, XIICPS_ISR_OFFSET);
+	}
+
+	/*
+	 * If there is an error, tell the caller.
+	 */
+	if (IntrStatusReg & Intrs) {
+		return XST_FAILURE;
+	}
+
+	return XST_SUCCESS;
+}
+
+/*****************************************************************************/
+/*
+* NOTE to MicroCART: This function is required by the send polling method above,
+* but it was originally static, so we had to copy it word-for-word here.
+*
+* This function prepares a device to transfers as a master.
+*
+* @param	InstancePtr is a pointer to the XIicPs instance.
+*
+* @param	Role specifies whether the device is sending or receiving.
+*
+* @return
+*		- XST_SUCCESS if everything went well.
+*		- XST_FAILURE if bus is busy.
+*
+* @note		Interrupts are always disabled, device which needs to use
+*		interrupts needs to setup interrupts after this call.
+*
+****************************************************************************/
+int XIicPs_SetupMaster(XIicPs *InstancePtr, int Role)
+{
+	u32 ControlReg;
+	u32 BaseAddr;
+	u32 EnabledIntr = 0x0;
+
+	Xil_AssertNonvoid(InstancePtr != NULL);
+
+	BaseAddr = InstancePtr->Config.BaseAddress;
+	ControlReg = XIicPs_ReadReg(BaseAddr, XIICPS_CR_OFFSET);
+
+
+	/*
+	 * Only check if bus is busy when repeated start option is not set.
+	 */
+	if ((ControlReg & XIICPS_CR_HOLD_MASK) == 0) {
+		if (XIicPs_BusIsBusy(InstancePtr)) {
+			return XST_FAILURE;
+		}
+	}
+
+	/*
+	 * Set up master, AckEn, nea and also clear fifo.
+	 */
+	ControlReg |= XIICPS_CR_ACKEN_MASK | XIICPS_CR_CLR_FIFO_MASK |
+		 	XIICPS_CR_NEA_MASK | XIICPS_CR_MS_MASK;
+
+	if (Role == RECVING_ROLE) {
+		ControlReg |= XIICPS_CR_RD_WR_MASK;
+		EnabledIntr = XIICPS_IXR_DATA_MASK |XIICPS_IXR_RX_OVR_MASK;
+	}else {
+		ControlReg &= ~XIICPS_CR_RD_WR_MASK;
+	}
+	EnabledIntr |= XIICPS_IXR_COMP_MASK | XIICPS_IXR_ARB_LOST_MASK;
+
+	XIicPs_WriteReg(BaseAddr, XIICPS_CR_OFFSET, ControlReg);
+
+	XIicPs_DisableAllInterrupts(BaseAddr);
+
+	return XST_SUCCESS;
+}