From 79eefd594f58a6416565ff25894964bd3c2a5afe Mon Sep 17 00:00:00 2001
From: Brendan Bartels <bbartels@iastate.edu>
Date: Sat, 21 Jan 2017 13:33:47 -0600
Subject: [PATCH] quad: re-implement communications.c to use a circular buffer

---
 quad/sw/modular_quad_pid/src/commands.c      | 137 ++++++++-----------
 quad/sw/modular_quad_pid/src/commands.h      |  58 ++++----
 quad/sw/modular_quad_pid/src/communication.c |  80 +++--------
 quad/sw/modular_quad_pid/src/communication.h |   1 +
 quad/sw/modular_quad_pid/src/uart_buff.c     |  97 ++++++++-----
 quad/sw/modular_quad_pid/src/uart_buff.h     |  13 +-
 6 files changed, 174 insertions(+), 212 deletions(-)

diff --git a/quad/sw/modular_quad_pid/src/commands.c b/quad/sw/modular_quad_pid/src/commands.c
index 471b09b68..c89696f2c 100644
--- a/quad/sw/modular_quad_pid/src/commands.c
+++ b/quad/sw/modular_quad_pid/src/commands.c
@@ -387,7 +387,7 @@ struct MessageType MessageTypes[MAX_TYPE] =
 	
 };
 
-int debug(unsigned char *packet, int dataLen, modular_structs_t *structs)
+int debug(modular_structs_t *structs)
 {
 	printf("function for debug\n");
 	return 0;
@@ -396,13 +396,13 @@ int debug(unsigned char *packet, int dataLen, modular_structs_t *structs)
 static int n_msg_received = 0;
 static size_t total_payload_received = 0;
 
-int packetlog(unsigned char* packet, int dataLen, modular_structs_t* structs) {
+int packetlog(modular_structs_t* structs) {
 	n_msg_received += 1;
-	total_payload_received += dataLen;
+	total_payload_received += uart_buff_data_length();
 	return 0;
 }
 
-int getpacketlogs(unsigned char* packet, int dataLen, modular_structs_t* structs) {
+int getpacketlogs(modular_structs_t* structs) {
 	char buf[255];
 
 	// Message logging number of messages received and size of payload received
@@ -413,25 +413,25 @@ int getpacketlogs(unsigned char* packet, int dataLen, modular_structs_t* structs
 }
 
 /* Handles receiving new location updates */
-int update(unsigned char *packet, int dataLen,	modular_structs_t *structs)
+int update(modular_structs_t *structs)
 {
 	//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 = getInt(packet, 0);
+	int packetId = uart_buff_data_get_int(0);
 //	printf("Packet ID: %d\n", packetId);
-	float y_pos = getFloat(packet, 4);
+	float y_pos = uart_buff_data_get_float(4);
 //	printf("y_pos: %f\n", y_pos);
-	float x_pos = getFloat(packet, 8);
+	float x_pos = uart_buff_data_get_float(8);
 //	printf("x_pos: %f\n", x_pos);
-	float alt_pos = getFloat(packet, 12);
+	float alt_pos = uart_buff_data_get_float(12);
 //	printf("alt_pos: %f\n", alt_pos);
-	float roll = getFloat(packet, 16);
+	float roll = uart_buff_data_get_float(16);
 //	printf("roll: %f\n", roll);
-	float pitch = getFloat(packet, 20);
+	float pitch = uart_buff_data_get_float(20);
 //	printf("pitch: %f\n", pitch);
-	float yaw = getFloat(packet, 24);
+	float yaw = uart_buff_data_get_float(24);
 //	printf("yaw: %f\n", yaw);
 
 	currentQuadPosition->packetId = packetId;
@@ -449,21 +449,24 @@ int update(unsigned char *packet, int dataLen,	modular_structs_t *structs)
 }
 
 // This is called on the ground station to begin sending VRPN to the quad
-int beginupdate(unsigned char *c, int dataLen, modular_structs_t *structs) {
+int beginupdate(modular_structs_t *structs) {
 	structs->user_input_struct.receivedBeginUpdate = 1;
 	return 0;
 }
 
-int logdata(unsigned char *packet, int dataLen, modular_structs_t *structs)
+int logdata(modular_structs_t *structs)
 {
+	char *packet = uart_buff_get_packet();
 	printf("Logging: %s\n", packet);
+	free(packet);
 	return 0;
 }
 
-int response(unsigned char *packet, int dataLen, modular_structs_t *structs)
+int response(modular_structs_t *structs)
 {
+	char *packet = uart_buff_get_packet();
 	printf("This is the response: %s\n", packet);
-	
+	free(packet);
 	return 0;
 }
 
@@ -472,16 +475,11 @@ int response(unsigned char *packet, int dataLen, modular_structs_t *structs)
 
 // TODO: Erase memory leaks
 
-int yawset(unsigned char *packet, int dataLen, modular_structs_t *structs)
+int yawset(modular_structs_t *structs)
 {
-	float value;
 	char buf[255] = {};
 	
-	// Copy the data into the value variable
-	memcpy(&value, ((float *)packet), dataLen);
-	
-	// Update the struct
-	structs->setpoint_struct.desiredQuadPosition.yaw = value;
+	structs->setpoint_struct.desiredQuadPosition.yaw = uart_buff_data_get_float(0);
 
 	// Debug print statement
 	//printf("function for yawset: %f\n", structs->setpoint_struct.desiredQuadPosition.yaw);
@@ -493,13 +491,11 @@ int yawset(unsigned char *packet, int dataLen, modular_structs_t *structs)
 	return 0;
 }
 
-int yawp(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+int yawp(modular_structs_t *structs)
 {
-	float value;
 	char buf[255] = {0};
-	
-	memcpy(&value, ((float *)packet), dataLen);
-	structs->parameter_struct.yaw_angle_pid.Kp = value;
+
+	structs->parameter_struct.yaw_angle_pid.Kp = uart_buff_data_get_float(0);
 	
 	printf("function for yawp: %f\n", structs->parameter_struct.yaw_angle_pid.Kp);
 	
@@ -510,13 +506,11 @@ int yawp(unsigned char *packet, int dataLen,  modular_structs_t *structs)
 	return 0;
 }
 
-int yawd(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+int yawd(modular_structs_t *structs)
 {
-	float value;
 	char buf[255] = {};
-	
-	memcpy(&value, ((float *)packet), dataLen);
-	structs->parameter_struct.yaw_angle_pid.Kd = value;
+
+	structs->parameter_struct.yaw_angle_pid.Kd = uart_buff_data_get_float(0);
 	
 	printf("function for yawd: %f\n", structs->parameter_struct.yaw_angle_pid.Kd);
 	
@@ -527,13 +521,11 @@ int yawd(unsigned char *packet, int dataLen,  modular_structs_t *structs)
 	return 0;
 }
 
-int rollset(unsigned char *packet, int dataLen,	 modular_structs_t *structs)
+int rollset(modular_structs_t *structs)
 {
-	float value;
 	char buf[255] = {};
 	
-	memcpy(&value, ((float *)packet), dataLen);
-	structs->setpoint_struct.desiredQuadPosition.roll = value;
+	structs->setpoint_struct.desiredQuadPosition.roll = uart_buff_data_get_float(0);
 	
 	printf("function for rollset: %f\n", structs->setpoint_struct.desiredQuadPosition.roll);
 	
@@ -544,12 +536,11 @@ int rollset(unsigned char *packet, int dataLen,	 modular_structs_t *structs)
 	return 0;
 }
 
-int rollp(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+int rollp(modular_structs_t *structs)
 {
-	float value;
 	char buf[255] = {};
-	memcpy(&value, ((float *)packet), dataLen);
-	structs->parameter_struct.roll_angle_pid.Kp = value;
+
+	structs->parameter_struct.roll_angle_pid.Kp = uart_buff_data_get_float(0);
 	
 	printf("function for rollp: %f\n", structs->parameter_struct.roll_angle_pid.Kp);
 	
@@ -561,13 +552,11 @@ int rollp(unsigned char *packet, int dataLen,  modular_structs_t *structs)
 	return 0;
 }
 
-int rolld(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+int rolld(modular_structs_t *structs)
 {
-	float value;
 	char buf[255] = {};
 	
-	memcpy(&value, ((float *)packet), dataLen);
-	structs->parameter_struct.roll_angle_pid.Kd = value;
+	structs->parameter_struct.roll_angle_pid.Kd = uart_buff_data_get_float(0);
 
 	printf("function for rolld: %f\n", structs->parameter_struct.roll_angle_pid.Kd);
 	
@@ -578,13 +567,11 @@ int rolld(unsigned char *packet, int dataLen,  modular_structs_t *structs)
 	return 0;
 }
 
-int pitchset(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+int pitchset(modular_structs_t *structs)
 {
-	float value;
 	char buf[255] = {};
 	
-	memcpy(&value, ((float *)packet), dataLen);
-	structs->setpoint_struct.desiredQuadPosition.pitch = value;
+	structs->setpoint_struct.desiredQuadPosition.pitch = uart_buff_data_get_float(0);
 	
 	printf("function for pitchset: %f\n", structs->setpoint_struct.desiredQuadPosition.pitch);
 	
@@ -595,13 +582,11 @@ int pitchset(unsigned char *packet, int dataLen,  modular_structs_t *structs)
 	return 0;
 }
 
-int pitchp(unsigned char *packet, int dataLen,	modular_structs_t *structs)
+int pitchp(modular_structs_t *structs)
 {
-	float value;
 	char buf[255] = {};
 	
-	memcpy(&value, ((float *)packet), dataLen);
-	structs->parameter_struct.pitch_angle_pid.Kp = value;
+	structs->parameter_struct.pitch_angle_pid.Kp = uart_buff_data_get_float(0);
 	
 	printf("function for pitchp: %f\n", structs->parameter_struct.pitch_angle_pid.Kp);
 	
@@ -612,13 +597,11 @@ int pitchp(unsigned char *packet, int dataLen,	modular_structs_t *structs)
 	return 0;
 }
 
-int pitchd(unsigned char *packet, int dataLen,	modular_structs_t *structs)
+int pitchd(modular_structs_t *structs)
 {
-	float value;
 	char buf[255] = {};
 	
-	memcpy(&value, ((float *)packet), dataLen);
-	structs->parameter_struct.pitch_angle_pid.Kd = value;
+	structs->parameter_struct.pitch_angle_pid.Kd = uart_buff_data_get_float(0);
 	
 	printf("function for pitchd: %f\n", structs->parameter_struct.pitch_angle_pid.Kd);
 	
@@ -631,13 +614,11 @@ int pitchd(unsigned char *packet, int dataLen,	modular_structs_t *structs)
 
 // ------------------------------------------------------------
 // These should be renamed to altitude!
-int throttleset(unsigned char *packet, int dataLen,	 modular_structs_t *structs)
+int throttleset(modular_structs_t *structs)
 {
-	float value;
 	char buf[255] = {};
 	
-	memcpy(&value, ((float *)packet), dataLen);
-	structs->setpoint_struct.desiredQuadPosition.alt_pos = value;
+	structs->setpoint_struct.desiredQuadPosition.alt_pos = uart_buff_data_get_float(0);
 	
 	printf("function for throttleset: %f\n", structs->setpoint_struct.desiredQuadPosition.alt_pos);
 	
@@ -648,13 +629,11 @@ int throttleset(unsigned char *packet, int dataLen,	 modular_structs_t *structs)
 	return 0;
 }
 
-int throttlep(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+int throttlep(modular_structs_t *structs)
 {
-	float value;
 	char buf[255] = {};
 	
-	memcpy(&value, ((float *)packet), dataLen);
-	structs->parameter_struct.alt_pid.Kp = value;
+	structs->parameter_struct.alt_pid.Kp = uart_buff_data_get_float(0);
 	
 	printf("function for throttlep: %f\n", structs->parameter_struct.alt_pid.Kp);
 	
@@ -665,13 +644,11 @@ int throttlep(unsigned char *packet, int dataLen,  modular_structs_t *structs)
 	return 0;
 }
 
-int throttlei(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+int throttlei(modular_structs_t *structs)
 {
-	float value;
 	char buf[255] = {};
 	
-	memcpy(&value, ((float *)packet), dataLen);
-	structs->parameter_struct.alt_pid.Ki = value;
+	structs->parameter_struct.alt_pid.Ki = uart_buff_data_get_float(0);
 	
 	printf("function for throttlei: %f\n", structs->parameter_struct.alt_pid.Ki);
 	
@@ -682,13 +659,11 @@ int throttlei(unsigned char *packet, int dataLen,  modular_structs_t *structs)
 	return 0;
 }
 
-int throttled(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+int throttled(modular_structs_t *structs)
 {
-	float value;
 	char buf[255] = {};
 	
-	memcpy(&value, ((float *)packet), dataLen);
-	structs->parameter_struct.alt_pid.Kd = value;
+	structs->parameter_struct.alt_pid.Kd = uart_buff_data_get_float(0);
 	
 	printf("function for throttled: %f\n", structs->parameter_struct.alt_pid.Kd);
 	
@@ -701,49 +676,49 @@ int throttled(unsigned char *packet, int dataLen,  modular_structs_t *structs)
 // These should be renamed to altitude!
 // ------------------------------------------------------------
 
-int accelreq(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+int accelreq(modular_structs_t *structs)
 {
 	printf("function for accelreq\n");
 	return 0;
 }
 
-int gyroresp(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+int gyroresp(modular_structs_t *structs)
 {
 	printf("function for accelreq\n");
 	return 0;
 }
 
-int pitchangleresp(unsigned char *packet, int dataLen,	modular_structs_t *structs)
+int pitchangleresp(modular_structs_t *structs)
 {
 	printf("function for accelreq\n");
 	return 0;
 }
 
-int rollangleresp(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+int rollangleresp(modular_structs_t *structs)
 {
 	printf("function for accelreq\n");
 	return 0;
 }
 
-int gyroreq(unsigned char *packet, int dataLen,	 modular_structs_t *structs)
+int gyroreq(modular_structs_t *structs)
 {
 	printf("function for accelreq\n");
 	return 0;
 }
 
-int pitchanglereq(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+int pitchanglereq(modular_structs_t *structs)
 {
 	printf("function for accelreq\n");
 	return 0;
 }
 
-int rollanglereq(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+int rollanglereq(modular_structs_t *structs)
 {
 	printf("function for accelreq\n");
 	return 0;
 }
 
-int accelresp(unsigned char *packet, int dataLen,  modular_structs_t *structs)
+int accelresp(modular_structs_t *structs)
 {
 	printf("function for accelreq\n");
 	return 0;
diff --git a/quad/sw/modular_quad_pid/src/commands.h b/quad/sw/modular_quad_pid/src/commands.h
index 45be51e8c..d93478dbf 100644
--- a/quad/sw/modular_quad_pid/src/commands.h
+++ b/quad/sw/modular_quad_pid/src/commands.h
@@ -31,7 +31,7 @@ struct MessageSubtype{
 	char ID;
 	char cmdText[100];
 	char cmdDataType;
-	int (*functionPtr)(unsigned char *command, int dataLen, modular_structs_t *structs);
+	int (*functionPtr)(modular_structs_t *structs);
 };
 
 // MESSAGE TYPES
@@ -40,34 +40,34 @@ struct MessageType{
 	struct MessageSubtype subtypes[MAX_SUBTYPE];
 };
 
-int debug(unsigned char *c, int dataLen, modular_structs_t *structs);
-int packetlog(unsigned char* packet, int dataLen, modular_structs_t* structs);
-int getpacketlogs(unsigned char* packet, int dataLen, modular_structs_t* structs);
-int update(unsigned char *c, int dataLen, modular_structs_t *structs);
-int beginupdate(unsigned char *c, int dataLen, modular_structs_t *structs);
-int logdata(unsigned char *c, int dataLen, modular_structs_t *structs);
-int response(unsigned char *packet, int dataLen, modular_structs_t *structs);
-int yawset(unsigned char *c, int dataLen, modular_structs_t *structs);
-int yawp(unsigned char *c, int dataLen, modular_structs_t *structs);
-int yawd(unsigned char *c, int dataLen, modular_structs_t *structs);
-int rollset(unsigned char *c, int dataLen, modular_structs_t *structs);
-int rollp(unsigned char *c, int dataLen, modular_structs_t *structs);
-int rolld(unsigned char *c, int dataLen, modular_structs_t *structs);
-int pitchset(unsigned char *c, int dataLen, modular_structs_t *structs);
-int pitchp(unsigned char *c, int dataLen, modular_structs_t *structs);
-int pitchd(unsigned char *c, int dataLen, modular_structs_t *structs);
-int throttleset(unsigned char *c, int dataLen, modular_structs_t *structs);
-int throttlep(unsigned char *c, int dataLen, modular_structs_t *structs);
-int throttlei(unsigned char *c, int dataLen, modular_structs_t *structs);
-int throttled(unsigned char *c, int dataLen, modular_structs_t *structs);
-int accelreq(unsigned char *c, int dataLen, modular_structs_t *structs);
-int gyroresp(unsigned char *c, int dataLen, modular_structs_t *structs);
-int pitchangleresp(unsigned char *c, int dataLen, modular_structs_t *structs);
-int rollangleresp(unsigned char *c, int dataLen, modular_structs_t *structs);
-int gyroreq(unsigned char *c, int dataLen, modular_structs_t *structs);
-int pitchanglereq(unsigned char *c, int dataLen, modular_structs_t *structs);
-int rollanglereq(unsigned char *c, int dataLen, modular_structs_t *structs);
-int accelresp(unsigned char *c, int dataLen, modular_structs_t *structs);
+int debug(modular_structs_t *structs);
+int packetlog(modular_structs_t* structs);
+int getpacketlogs(modular_structs_t* structs);
+int update(modular_structs_t *structs);
+int beginupdate(modular_structs_t *structs);
+int logdata(modular_structs_t *structs);
+int response(modular_structs_t *structs);
+int yawset(modular_structs_t *structs);
+int yawp(modular_structs_t *structs);
+int yawd(modular_structs_t *structs);
+int rollset(modular_structs_t *structs);
+int rollp(modular_structs_t *structs);
+int rolld(modular_structs_t *structs);
+int pitchset(modular_structs_t *structs);
+int pitchp(modular_structs_t *structs);
+int pitchd(modular_structs_t *structs);
+int throttleset(modular_structs_t *structs);
+int throttlep(modular_structs_t *structs);
+int throttlei(modular_structs_t *structs);
+int throttled(modular_structs_t *structs);
+int accelreq(modular_structs_t *structs);
+int gyroresp(modular_structs_t *structs);
+int pitchangleresp(modular_structs_t *structs);
+int rollangleresp(modular_structs_t *structs);
+int gyroreq(modular_structs_t *structs);
+int pitchanglereq(modular_structs_t *structs);
+int rollanglereq(modular_structs_t *structs);
+int accelresp(modular_structs_t *structs);
 
 // TODO add in string to be read from the command line when sending a subtype of message
 extern struct MessageType MessageTypes[MAX_TYPE];
diff --git a/quad/sw/modular_quad_pid/src/communication.c b/quad/sw/modular_quad_pid/src/communication.c
index bc8a1df43..11935385d 100644
--- a/quad/sw/modular_quad_pid/src/communication.c
+++ b/quad/sw/modular_quad_pid/src/communication.c
@@ -1,4 +1,5 @@
 #include "communication.h"
+#include "uart_buff.h"
 
 #define INTC		XScuGic
 #define COMM_UART_DEVICE_ID		XPAR_PS7_UART_0_DEVICE_ID
@@ -19,10 +20,6 @@ void Handler(void *CallBackRef, u32 Event, unsigned int EventData);
 // Pointer to the UART driver instance
 static XUartPs* uartInstPtr;
 
-static u8 recvBuf[UART_BUF_SIZE];
-static volatile size_t recv_buf_begin = 0; // Index of start of valid packets in buffer
-static volatile size_t recv_buf_end = 0; // One past end of valid packets in buffer
-
 
 int initUartComms() {
 	uartInstPtr = uart0_init(COMM_UART_DEVICE_ID, BAUD_RATE);
@@ -70,22 +67,22 @@ int initUartComms() {
 	return 0;
 }
 
-int process_packet(unsigned char* packet, modular_structs_t *structs) {
+int process_packet(modular_structs_t *structs) {
 	metadata_t meta_data;
 	// parse metadata
-	meta_data.begin_char = packet[0];
-	meta_data.msg_type = packet[1];
-	meta_data.msg_subtype = packet[2];
-	meta_data.msg_id = (packet[4] << 8) | (packet[3]);
-	meta_data.data_len = (packet[6] << 8) | (packet[5]);
-	unsigned char packet_checksum = packet[7+meta_data.data_len];
-	unsigned char* packet_data = packet + sizeof(metadata_t);
+	meta_data.begin_char = uart_buff_get_byte(0);
+	meta_data.msg_type = uart_buff_get_byte(01);
+	meta_data.msg_subtype = uart_buff_get_byte(2);
+	meta_data.msg_id = (uart_buff_get_byte(4) << 8) | (uart_buff_get_byte(3));
+	meta_data.data_len = (uart_buff_get_byte(6) << 8) | (uart_buff_get_byte(5));
+	unsigned char packet_checksum = uart_buff_get_byte(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 ^= packet[i];
+		calculated_checksum ^= uart_buff_get_byte(i);
 	}
 	// Discard if checksum didn't match
 	if(packet_checksum != calculated_checksum) {
@@ -94,39 +91,12 @@ int process_packet(unsigned char* packet, modular_structs_t *structs) {
 	}
 
 	// Call appropriate function for packet
-	(* (MessageTypes[meta_data.msg_type].subtypes[meta_data.msg_subtype].functionPtr))(packet_data, meta_data.data_len, structs);
+	(* (MessageTypes[meta_data.msg_type].subtypes[meta_data.msg_subtype].functionPtr))(structs);
 
-	return 0;
+	uart_buff_flush_packet();
 
-}
+	return 0;
 
-void parse_available(modular_structs_t *structs) {
-	// Minimum size of a packet (header + checksum)
-	int min_packet_size = 8;
-	// TODO: Loop limits?
-	while (recv_buf_end - recv_buf_begin >= min_packet_size) {
-		// Discard all data before packet begin character
-		while (recv_buf_begin < recv_buf_end && recvBuf[recv_buf_begin] != BEGIN_CHAR) {
-			recv_buf_begin++;
-		}
-		// If, after discarding everything before header, not enough is left, don't parse
-		if (recv_buf_end - recv_buf_begin < min_packet_size) {
-			break;
-		}
-		unsigned char* packet_header = recvBuf + recv_buf_begin;
-
-		// Read out the payload size information from the header
-		size_t packet_len = (packet_header[6] << 8) | (packet_header[5]);
-		// Determine if we have received the entire packet. If so, process it.
-		if (recv_buf_end - recv_buf_begin >= min_packet_size + packet_len) {
-			process_packet(recvBuf + recv_buf_begin, structs);
-			recv_buf_begin += min_packet_size + packet_len;
-		}
-		else {
-			// Not enough data for a packet
-			break;
-		}
-	}
 }
 
 /*
@@ -148,21 +118,9 @@ void restore_interrupts(u32 intr_state) {
 
 void process_received(modular_structs_t *structs) {
 	// Parse as many packets as possible
-	parse_available(structs);
-
-	// Disable interrupts while moving data around
-	u32 intr_state = disable_interrupts();
-
-	// Move unprocessed bytes to front of secondary buffer
-	size_t unprocessed_size = recv_buf_end - recv_buf_begin;
-	memmove(recvBuf, recvBuf + recv_buf_begin, unprocessed_size);
-	recv_buf_begin = 0;
-	recv_buf_end = unprocessed_size;
-
-	restore_interrupts(intr_state);
-	//XUartPs_SetRecvTimeout(uartInstPtr, 8);
-	//unsigned char in_fifo = XUartPs_ReadReg(uartInstPtr->Config.BaseAddress, XUARTPS_FIFO_OFFSET);
-	return;
+	while (uart_buff_packet_ready()) {
+		process_packet(structs);
+	}
 }
 
 void uart_interrupt_handler(XUartPs *InstancePtr) {
@@ -186,9 +144,9 @@ void uart_interrupt_handler(XUartPs *InstancePtr) {
 	u32 CsrRegister = XUartPs_ReadReg(InstancePtr->Config.BaseAddress,
 				XUARTPS_SR_OFFSET);
 
-	while (0 == (CsrRegister & XUARTPS_SR_RXEMPTY) && recv_buf_end <= UART_BUF_SIZE) {
-		recvBuf[recv_buf_end] = XUartPs_ReadReg(InstancePtr->Config.BaseAddress, XUARTPS_FIFO_OFFSET);
-		recv_buf_end += 1;
+	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);
 	}
 
diff --git a/quad/sw/modular_quad_pid/src/communication.h b/quad/sw/modular_quad_pid/src/communication.h
index df87802cc..12516af08 100644
--- a/quad/sw/modular_quad_pid/src/communication.h
+++ b/quad/sw/modular_quad_pid/src/communication.h
@@ -11,6 +11,7 @@
 #include "mio7_led.h"
 #include "timer.h"
 #include "commands.h"
+#include "uart_buff.h"
 
 int initUartComms();
 void process_received(modular_structs_t *structs);
diff --git a/quad/sw/modular_quad_pid/src/uart_buff.c b/quad/sw/modular_quad_pid/src/uart_buff.c
index 4841e58e8..a11a78f47 100644
--- a/quad/sw/modular_quad_pid/src/uart_buff.c
+++ b/quad/sw/modular_quad_pid/src/uart_buff.c
@@ -1,35 +1,28 @@
-#include <stddef.h>
 #include <stdio.h>
 #include "uart_buff.h"
+#include <stdlib.h>
 
-/* #define UART_MAX_BUFF_SIZE 2048 */
-/* #define UART_MAX_PACKET_SIZE 256 */
-#define UART_MAX_BUFF_SIZE 20
-#define UART_MAX_PACKET_SIZE 10
+#define UART_MAX_BUFF_SIZE 2048
+#define UART_MAX_PACKET_SIZE 256
 
-static unsigned char buff[UART_MAX_BUFF_SIZE];
+static volatile unsigned char buff[UART_MAX_BUFF_SIZE];
 static size_t start = UART_MAX_BUFF_SIZE;
-static size_t packet_size = 0;
-static size_t end = 0;
+static size_t packet_data_length = 0;
+static volatile size_t end = 0;
 static int packet_start_found = 0;
 static int packet_size_found = 0;
 static int packet_ready = 0;
 
 /**
- * Put a byte into the buffer. Afterwards, a packet scan will be performed
- * to update any necessary indices and flags.
+ * Put a byte into the buffer.
  */
-void uart_buff_put_byte(unsigned char c) {
+void uart_buff_put_byte(u8 c) {
   if (uart_buff_full()) {
     return;
   }
 
   buff[end] = c;
-  uart_buff_incr_index(&end);
-
-  if (!packet_ready) {
-    uart_buff_scan_packet();
-  }
+  end = uart_buff_calc_index(end + 1);
 }
 
 /**
@@ -46,7 +39,7 @@ void uart_buff_scan_packet() {
 
   if (packet_start_found
       && packet_size_found
-      && (uart_buff_size() >= packet_size)) {
+      && (uart_buff_size() >= uart_buff_packet_size())) {
     packet_ready = 1;
   }
 }
@@ -57,7 +50,7 @@ void uart_buff_scan_packet() {
  */
 void uart_buff_scan_packet_start() {
   while (buff[start] != 0xBE && !uart_buff_empty()) {
-    uart_buff_incr_index(&start);
+    start = uart_buff_calc_index(start + 1);
   }
   if (buff[start] == 0xBE) {
     packet_start_found = 1;
@@ -73,11 +66,10 @@ void uart_buff_scan_packet_size() {
     return;
   }
 
-  size_t data_len = (uart_buff_get_byte(6) << 8) | uart_buff_get_byte(5);
-  packet_size = 1 + 1 + 1 + 2 + data_len + 1;
+  packet_data_length = (uart_buff_get_byte(6) << 8) | uart_buff_get_byte(5);
   packet_size_found = 1;
 
-  if (packet_size > UART_MAX_PACKET_SIZE) {
+  if (uart_buff_packet_size() > UART_MAX_PACKET_SIZE) {
     // Packet size is too big. Abort this packet.
     uart_buff_reset_flags();
     start += 1;
@@ -88,6 +80,9 @@ void uart_buff_scan_packet_size() {
  * Return 1 if a packet is ready to processed, 0 otherwise.
  */
 int uart_buff_packet_ready() {
+  if (!packet_ready) {
+    uart_buff_scan_packet();
+  }
   return packet_ready;
 }
 
@@ -95,7 +90,7 @@ int uart_buff_packet_ready() {
  * Retrieve a byte from the buffer according to the given offset with respect
  * to the start index of the buffer.
  */
-unsigned char uart_buff_get_byte(size_t offset) {
+u8 uart_buff_get_byte(size_t offset) {
   size_t target = uart_buff_calc_index(start + offset);
 
   return buff[target];
@@ -107,7 +102,7 @@ unsigned char uart_buff_get_byte(size_t offset) {
  *
  * This has undefined behavior if a packet is not ready.
  */
-unsigned char uart_buff_data_get_byte(size_t offset) {
+u8 uart_buff_data_get_byte(size_t offset) {
   return uart_buff_get_byte(7 + offset);
 }
 
@@ -130,11 +125,17 @@ int uart_buff_data_get_int(size_t offset) {
  * packet.
  *
  * This has undefined behavior if a packet is not ready.
-
  */
 float uart_buff_data_get_float(size_t offset) {
-  // TODO
-  return 0;
+	union {
+		float f;
+		int i;
+	} x;
+	x.i = uart_buff_data_get_byte(offset + 3) << 24 |
+			uart_buff_data_get_byte(offset + 2) << 16 |
+			uart_buff_data_get_byte(offset + 1) << 8 |
+			uart_buff_data_get_byte(offset);
+	return x.f;
 }
 
 /**
@@ -146,27 +147,33 @@ void uart_buff_flush_packet() {
     return;
   }
 
-  start = uart_buff_calc_index(start + packet_size + 2);
+  start = uart_buff_calc_index(start + uart_buff_packet_size() + 1);
   uart_buff_reset_flags();
-  uart_buff_scan_packet();
 }
 
 /**
- * Increment the given index, wrapping around the edges of the circular buffer.
+ * Return the current number of bytes held in the buffer.
  */
-void uart_buff_incr_index(size_t *index) {
-  *index = uart_buff_calc_index(*index + 1);
+size_t uart_buff_size() {
+  return uart_buff_calc_index(end - start);
 }
 
 /**
- * Return the current number of bytes held in the buffer.
+ * Return the length of the data portion of the current packet.
  */
-size_t uart_buff_size() {
-  return uart_buff_calc_index(end - start);
+size_t uart_buff_data_length() {
+  return packet_data_length;
+}
+
+/**
+ * Return the size of the current packet.
+ */
+size_t uart_buff_packet_size() {
+  return 1 + 1 + 1 + 2 + packet_data_length + 1;
 }
 
 /**
- * Reset all flags indicating the status of scanned packets.
+ * Reset all flags indicating the NOTEstatus of scanned packets.
  */
 void uart_buff_reset_flags() {
   packet_start_found = 0;
@@ -183,7 +190,7 @@ int uart_buff_empty() {
 }
 
 /**
- * Return 1 if the buffer is fully, 0 otherwise.
+ * Return 1 if the buffer is full, 0 otherwise.
  */
 int uart_buff_full() {
   return start == end;
@@ -204,10 +211,28 @@ size_t uart_buff_calc_index(int given_index) {
   return actual_index;
 }
 
+/**
+ * Return a string pointer to the start of the data section.
+ *
+ * ALERT! Consumers of this function must free the pointer they received,
+ * or else they will introduce a memory leak!
+ */
+char * uart_buff_get_packet() {
+	size_t packet_size = uart_buff_packet_size();
+	char *str = malloc(sizeof(char) * (packet_size + 1));
+	str[packet_size + 1] = '\0'; // null terminate string
+	int i;
+	for (i = 0; i < packet_size; i += 1) {
+		str[i] = uart_buff_get_byte(i);
+	}
+	return str;
+}
+
 /**
  * Print the buffer with a pretty ASCII art image.
  */
 void uart_buff_print() {
+  size_t packet_size = uart_buff_packet_size();
   int i;
   printf("buffer size = %zu\n", uart_buff_size());
   puts("Buffer:");
diff --git a/quad/sw/modular_quad_pid/src/uart_buff.h b/quad/sw/modular_quad_pid/src/uart_buff.h
index ae229dad5..7dcd0fa69 100644
--- a/quad/sw/modular_quad_pid/src/uart_buff.h
+++ b/quad/sw/modular_quad_pid/src/uart_buff.h
@@ -1,24 +1,27 @@
 #ifndef CIRC_BUFF_H
 #define CIRC_BUFF_H
 
-#include <stddef.h>
+#include "xil_types.h"
 
-void uart_buff_put_byte(unsigned char);
+void uart_buff_put_byte(u8);
 void uart_buff_scan_packet();
 void uart_buff_scan_packet_start();
 void uart_buff_scan_packet_size();
 int uart_buff_packet_ready();
-unsigned char uart_buff_get_byte(size_t);
-unsigned char uart_buff_data_get_byte(size_t);
+u8 uart_buff_get_byte(size_t);
+u8 uart_buff_data_get_byte(size_t);
 int uart_buff_data_get_int(size_t);
 float uart_buff_data_get_float(size_t);
 void uart_buff_flush_packet();
-void uart_buff_incr_index(size_t *);
 size_t uart_buff_size();
+size_t uart_buff_data_length();
+size_t uart_buff_packet_size();
 void uart_buff_reset_flags();
 int uart_buff_empty();
 int uart_buff_full();
 size_t uart_buff_calc_index(int);
+char * uart_buff_get_packet();
 void uart_buff_print();
 
+
 #endif
-- 
GitLab