From 4b2bed4df7cafb44ea2d3b9979cf4e7743fa6a11 Mon Sep 17 00:00:00 2001
From: "ucart@co3050-12" <co3050-12@iastate.edu>
Date: Thu, 19 Jan 2017 19:48:43 -0600
Subject: [PATCH] Integrated changes from comm_dev (interrupt-driven UART) and
 updated code to work well with the new style. Compiles, but have not tested
 running yet.

---
 quad/sw/modular_quad_pid/src/commands.c       | 342 +++-------------
 quad/sw/modular_quad_pid/src/commands.h       |   1 +
 quad/sw/modular_quad_pid/src/communication.c  | 377 +++++++++++-------
 quad/sw/modular_quad_pid/src/communication.h  |  22 +-
 .../modular_quad_pid/src/control_algorithm.c  |  17 +-
 .../src/initialize_components.c               |  67 +---
 .../src/initialize_components.h               |   2 +-
 quad/sw/modular_quad_pid/src/log_data.c       |  53 +--
 quad/sw/modular_quad_pid/src/main.c           |   5 +-
 .../modular_quad_pid/src/packet_processing.c  |  37 --
 quad/sw/modular_quad_pid/src/sensor.c         |   5 -
 quad/sw/modular_quad_pid/src/stringBuilder.c  | 199 ---------
 quad/sw/modular_quad_pid/src/stringBuilder.h  |  59 ---
 quad/sw/modular_quad_pid/src/timer.c          |   4 +-
 quad/sw/modular_quad_pid/src/type_def.h       |  25 +-
 quad/sw/modular_quad_pid/src/uart.c           |  71 ++++
 quad/sw/modular_quad_pid/src/uart.h           |  10 +-
 quad/sw/modular_quad_pid/src/user_input.h     |   1 -
 18 files changed, 397 insertions(+), 900 deletions(-)
 delete mode 100644 quad/sw/modular_quad_pid/src/stringBuilder.c
 delete mode 100644 quad/sw/modular_quad_pid/src/stringBuilder.h

diff --git a/quad/sw/modular_quad_pid/src/commands.c b/quad/sw/modular_quad_pid/src/commands.c
index 97ed7720a..3f2a62e83 100644
--- a/quad/sw/modular_quad_pid/src/commands.c
+++ b/quad/sw/modular_quad_pid/src/commands.c
@@ -371,16 +371,45 @@ int debug(unsigned char *packet, int dataLen, modular_structs_t *structs)
 	return 0;
 }
 
-/* This is not used. Many flow changes would be need to be made to the
-	control algorithm in order to get this to work. */
+/* Handles receiving new location updates */
 int update(unsigned char *packet, int dataLen,	modular_structs_t *structs)
 {
-	printf("function for debug\n");
+	//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);
+//	printf("Packet ID: %d\n", packetId);
+	float y_pos = getFloat(packet, 4);
+//	printf("y_pos: %f\n", y_pos);
+	float x_pos = getFloat(packet, 8);
+//	printf("x_pos: %f\n", x_pos);
+	float alt_pos = getFloat(packet, 12);
+//	printf("alt_pos: %f\n", alt_pos);
+	float roll = getFloat(packet, 16);
+//	printf("roll: %f\n", roll);
+	float pitch = getFloat(packet, 20);
+//	printf("pitch: %f\n", pitch);
+	float yaw = getFloat(packet, 24);
+//	printf("yaw: %f\n", yaw);
+
+	currentQuadPosition->packetId = packetId;
+	currentQuadPosition->y_pos = y_pos;
+	currentQuadPosition->x_pos = x_pos;
+	currentQuadPosition->alt_pos = alt_pos;
+	currentQuadPosition->roll = roll;
+	currentQuadPosition->pitch = pitch;
+	currentQuadPosition->yaw = yaw;
+
+	// Make location as fresh
+	structs->user_input_struct.locationFresh = 1;
+
 	return 0;
 }
 
 // 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) {
+	structs->user_input_struct.receivedBeginUpdate = 1;
 	return 0;
 }
 
@@ -406,7 +435,6 @@ int yawset(unsigned char *packet, int dataLen, modular_structs_t *structs)
 {
 	float value;
 	char buf[255] = {};
-	int i;
 	
 	// Copy the data into the value variable
 	memcpy(&value, ((float *)packet), dataLen);
@@ -418,29 +446,9 @@ int yawset(unsigned char *packet, int dataLen, modular_structs_t *structs)
 	//printf("function for yawset: %f\n", structs->setpoint_struct.desiredQuadPosition.yaw);
 	
 	// Send a reply to the ground station
-	snprintf(buf, sizeof(buf), "Successfully set desired yaw to %.2f radians\r\n", structs->setpoint_struct.desiredQuadPosition.yaw);
-	unsigned char *responsePacket;
-
-	printf("%s\n", buf);
-
-	metadata_t metadata =
-	{
-		BEGIN_CHAR,
-		MessageTypes[5].ID,
-		MessageTypes[5].subtypes[1].ID,
-		0,
-		(strlen(buf) + 1)
-	};
-	formatPacket(&metadata, buf, &responsePacket);
-
-	// Send each byte of the packet individually
-	for(i = 0; i < 8 + metadata.data_len; i++) {
-		// Debug print statement for all of the bytes being sent
-		printf("%d: 0x%x\n", i, responsePacket[i]);
-
-		uart0_sendByte(responsePacket[i]);
-	}
+	int length = snprintf(buf, sizeof(buf), "Successfully set desired yaw to %.2f radians\r\n", structs->setpoint_struct.desiredQuadPosition.yaw);
 
+	send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
 	return 0;
 }
 
@@ -448,7 +456,6 @@ int yawp(unsigned char *packet, int dataLen,  modular_structs_t *structs)
 {
 	float value;
 	char buf[255] = {0};
-	int i;
 	
 	memcpy(&value, ((float *)packet), dataLen);
 	structs->parameter_struct.yaw_angle_pid.Kp = value;
@@ -456,29 +463,9 @@ int yawp(unsigned char *packet, int dataLen,  modular_structs_t *structs)
 	printf("function for yawp: %f\n", structs->parameter_struct.yaw_angle_pid.Kp);
 	
 	// Send a reply to the ground station
-	snprintf(buf, sizeof(buf), "Successfully set yaw Kp to %.2f\r\n", structs->parameter_struct.yaw_angle_pid.Kp);
-	unsigned char *responsePacket;
-
-	printf("%s\n", buf);
-
-	metadata_t metadata =
-	{
-		BEGIN_CHAR,
-		MessageTypes[5].ID,
-		MessageTypes[5].subtypes[1].ID,
-		0,
-		(strlen(buf) + 1)
-	};
-	formatPacket(&metadata, buf, &responsePacket);
-
-	// Send each byte of the packet individually
-	for(i = 0; i < 8 + metadata.data_len; i++) {
-		// Debug print statement for all of the bytes being sent
-		//printf("%d: 0x%x\n", i, packet2[i]);
-
-		uart0_sendByte(responsePacket[i]);
-	}
+	int length = snprintf(buf, sizeof(buf), "Successfully set yaw Kp to %.2f\r\n", structs->parameter_struct.yaw_angle_pid.Kp);
 
+	send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
 	return 0;
 }
 
@@ -486,7 +473,6 @@ int yawd(unsigned char *packet, int dataLen,  modular_structs_t *structs)
 {
 	float value;
 	char buf[255] = {};
-	int i;
 	
 	memcpy(&value, ((float *)packet), dataLen);
 	structs->parameter_struct.yaw_angle_pid.Kd = value;
@@ -494,29 +480,9 @@ int yawd(unsigned char *packet, int dataLen,  modular_structs_t *structs)
 	printf("function for yawd: %f\n", structs->parameter_struct.yaw_angle_pid.Kd);
 	
 	// Send a reply to the ground station
-	snprintf(buf, sizeof(buf), "Successfully set yaw Kd to %.2f\r\n", structs->parameter_struct.yaw_angle_pid.Kd);
-	unsigned char *responsePacket;
-
-	printf("%s\n", buf);
-
-	metadata_t metadata =
-	{
-		BEGIN_CHAR,
-		MessageTypes[5].ID,
-		MessageTypes[5].subtypes[1].ID,
-		0,
-		(strlen(buf) + 1)
-	};
-	formatPacket(&metadata, buf, &responsePacket);
-
-	// Send each byte of the packet individually
-	for(i = 0; i < 8 + metadata.data_len; i++) {
-		// Debug print statement for all of the bytes being sent
-		//printf("%d: 0x%x\n", i, responsePacket[i]);
-
-		uart0_sendByte(responsePacket[i]);
-	}
+	int length = snprintf(buf, sizeof(buf), "Successfully set yaw Kd to %.2f\r\n", structs->parameter_struct.yaw_angle_pid.Kd);
 
+	send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
 	return 0;
 }
 
@@ -524,7 +490,6 @@ int rollset(unsigned char *packet, int dataLen,	 modular_structs_t *structs)
 {
 	float value;
 	char buf[255] = {};
-	int i;
 	
 	memcpy(&value, ((float *)packet), dataLen);
 	structs->setpoint_struct.desiredQuadPosition.roll = value;
@@ -532,27 +497,9 @@ int rollset(unsigned char *packet, int dataLen,	 modular_structs_t *structs)
 	printf("function for rollset: %f\n", structs->setpoint_struct.desiredQuadPosition.roll);
 	
 	// Send a reply to the ground station
-	snprintf(buf, sizeof(buf), "Successfully set desired roll to %.2f radians\r\n", structs->setpoint_struct.desiredQuadPosition.roll);
-	unsigned char *responsePacket;
-
-	metadata_t metadata =
-	{
-		BEGIN_CHAR,
-		MessageTypes[5].ID,
-		MessageTypes[5].subtypes[1].ID,
-		0,
-		(strlen(buf) + 1)
-	};
-	formatPacket(&metadata, buf, &responsePacket);
-
-	// Send each byte of the packet individually
-	for(i = 0; i < 8 + metadata.data_len; i++) {
-		// Debug print statement for all of the bytes being sent
-		//printf("%d: 0x%x\n", i, responsePacket[i]);
-
-		uart0_sendByte(responsePacket[i]);
-	}
+	int length = snprintf(buf, sizeof(buf), "Successfully set desired roll to %.2f radians\r\n", structs->setpoint_struct.desiredQuadPosition.roll);
 
+	send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
 	return 0;
 }
 
@@ -560,34 +507,15 @@ int rollp(unsigned char *packet, int dataLen,  modular_structs_t *structs)
 {
 	float value;
 	char buf[255] = {};
-	int i;
-	
 	memcpy(&value, ((float *)packet), dataLen);
 	structs->parameter_struct.roll_angle_pid.Kp = value;
 	
 	printf("function for rollp: %f\n", structs->parameter_struct.roll_angle_pid.Kp);
 	
 	// Send a reply to the ground station
-	snprintf(buf, sizeof(buf), "Successfully set roll Kp to %.2f\r\n", structs->parameter_struct.roll_angle_pid.Kp);
-	unsigned char *responsePacket;
+	size_t length = snprintf(buf, sizeof(buf), "Successfully set roll Kp to %.2f\r\n", structs->parameter_struct.roll_angle_pid.Kp);
 
-	metadata_t metadata =
-	{
-		BEGIN_CHAR,
-		MessageTypes[5].ID,
-		MessageTypes[5].subtypes[1].ID,
-		0,
-		(strlen(buf) + 1)
-	};
-	formatPacket(&metadata, buf, &responsePacket);
-
-	// Send each byte of the packet individually
-	for(i = 0; i < 8 + metadata.data_len; i++) {
-		// Debug print statement for all of the bytes being sent
-		//printf("%d: 0x%x\n", i, responsePacket[i]);
-
-		uart0_sendByte(responsePacket[i]);
-	}
+	send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
 
 	return 0;
 }
@@ -596,35 +524,16 @@ int rolld(unsigned char *packet, int dataLen,  modular_structs_t *structs)
 {
 	float value;
 	char buf[255] = {};
-	int i;
 	
 	memcpy(&value, ((float *)packet), dataLen);
 	structs->parameter_struct.roll_angle_pid.Kd = value;
-	
+
 	printf("function for rolld: %f\n", structs->parameter_struct.roll_angle_pid.Kd);
 	
 	// Send a reply to the ground station
-	snprintf(buf, sizeof(buf), "Successfully set roll Kd to %.2f\r\n", structs->parameter_struct.roll_angle_pid.Kd);
-	unsigned char *responsePacket;
-
-	metadata_t metadata =
-	{
-		BEGIN_CHAR,
-		MessageTypes[5].ID,
-		MessageTypes[5].subtypes[1].ID,
-		0,
-		(strlen(buf) + 1)
-	};
-	formatPacket(&metadata, buf, &responsePacket);
-
-	// Send each byte of the packet individually
-	for(i = 0; i < 8 + metadata.data_len; i++) {
-		// Debug print statement for all of the bytes being sent
-		//printf("%d: 0x%x\n", i, responsePacket[i]);
-
-		uart0_sendByte(responsePacket[i]);
-	}
+	size_t length = snprintf(buf, sizeof(buf), "Successfully set roll Kd to %.2f\r\n", structs->parameter_struct.roll_angle_pid.Kd);
 
+	send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
 	return 0;
 }
 
@@ -632,7 +541,6 @@ int pitchset(unsigned char *packet, int dataLen,  modular_structs_t *structs)
 {
 	float value;
 	char buf[255] = {};
-	int i;
 	
 	memcpy(&value, ((float *)packet), dataLen);
 	structs->setpoint_struct.desiredQuadPosition.pitch = value;
@@ -640,27 +548,9 @@ int pitchset(unsigned char *packet, int dataLen,  modular_structs_t *structs)
 	printf("function for pitchset: %f\n", structs->setpoint_struct.desiredQuadPosition.pitch);
 	
 	// Send a reply to the ground station
-	snprintf(buf, sizeof(buf), "Successfully set desired pitch to %.2f radians\r\n", structs->setpoint_struct.desiredQuadPosition.pitch);
-	unsigned char *responsePacket;
-
-	metadata_t metadata =
-	{
-		BEGIN_CHAR,
-		MessageTypes[5].ID,
-		MessageTypes[5].subtypes[1].ID,
-		0,
-		(strlen(buf) + 1)
-	};
-	formatPacket(&metadata, buf, &responsePacket);
-
-	// Send each byte of the packet individually
-	for(i = 0; i < 8 + metadata.data_len; i++) {
-		// Debug print statement for all of the bytes being sent
-		//printf("%d: 0x%x\n", i, responsePacket[i]);
-
-		uart0_sendByte(responsePacket[i]);
-	}
+	int length = snprintf(buf, sizeof(buf), "Successfully set desired pitch to %.2f radians\r\n", structs->setpoint_struct.desiredQuadPosition.pitch);
 
+	send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
 	return 0;
 }
 
@@ -668,7 +558,6 @@ int pitchp(unsigned char *packet, int dataLen,	modular_structs_t *structs)
 {
 	float value;
 	char buf[255] = {};
-	int i;
 	
 	memcpy(&value, ((float *)packet), dataLen);
 	structs->parameter_struct.pitch_angle_pid.Kp = value;
@@ -676,27 +565,9 @@ int pitchp(unsigned char *packet, int dataLen,	modular_structs_t *structs)
 	printf("function for pitchp: %f\n", structs->parameter_struct.pitch_angle_pid.Kp);
 	
 	// Send a reply to the ground station
-	snprintf(buf, sizeof(buf), "Successfully set pitch Kp to %.2f\r\n", structs->parameter_struct.pitch_angle_pid.Kp);
-	unsigned char *responsePacket;
-
-	metadata_t metadata =
-	{
-		BEGIN_CHAR,
-		MessageTypes[5].ID,
-		MessageTypes[5].subtypes[1].ID,
-		0,
-		(strlen(buf) + 1)
-	};
-	formatPacket(&metadata, buf, &responsePacket);
-
-	// Send each byte of the packet individually
-	for(i = 0; i < 8 + metadata.data_len; i++) {
-		// Debug print statement for all of the bytes being sent
-		//printf("%d: 0x%x\n", i, responsePacket[i]);
-
-		uart0_sendByte(responsePacket[i]);
-	}
+	int length = snprintf(buf, sizeof(buf), "Successfully set pitch Kp to %.2f\r\n", structs->parameter_struct.pitch_angle_pid.Kp);
 
+	send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
 	return 0;
 }
 
@@ -704,7 +575,6 @@ int pitchd(unsigned char *packet, int dataLen,	modular_structs_t *structs)
 {
 	float value;
 	char buf[255] = {};
-	int i;
 	
 	memcpy(&value, ((float *)packet), dataLen);
 	structs->parameter_struct.pitch_angle_pid.Kd = value;
@@ -712,27 +582,9 @@ int pitchd(unsigned char *packet, int dataLen,	modular_structs_t *structs)
 	printf("function for pitchd: %f\n", structs->parameter_struct.pitch_angle_pid.Kd);
 	
 	// Send a reply to the ground station
-	snprintf(buf, sizeof(buf), "Successfully set desired yaw to %.2f\r\n", structs->parameter_struct.pitch_angle_pid.Kd);
-	unsigned char *responsePacket;
-
-	metadata_t metadata =
-	{
-		BEGIN_CHAR,
-		MessageTypes[5].ID,
-		MessageTypes[5].subtypes[1].ID,
-		0,
-		(strlen(buf) + 1)
-	};
-	formatPacket(&metadata, buf, &responsePacket);
-
-	// Send each byte of the packet individually
-	for(i = 0; i < 8 + metadata.data_len; i++) {
-		// Debug print statement for all of the bytes being sent
-		//printf("%d: 0x%x\n", i, responsePacket[i]);
-
-		uart0_sendByte(responsePacket[i]);
-	}
+	int length = snprintf(buf, sizeof(buf), "Successfully set desired yaw to %.2f\r\n", structs->parameter_struct.pitch_angle_pid.Kd);
 
+	send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
 	return 0;
 }
 
@@ -742,7 +594,6 @@ int throttleset(unsigned char *packet, int dataLen,	 modular_structs_t *structs)
 {
 	float value;
 	char buf[255] = {};
-	int i;
 	
 	memcpy(&value, ((float *)packet), dataLen);
 	structs->setpoint_struct.desiredQuadPosition.alt_pos = value;
@@ -750,27 +601,9 @@ int throttleset(unsigned char *packet, int dataLen,	 modular_structs_t *structs)
 	printf("function for throttleset: %f\n", structs->setpoint_struct.desiredQuadPosition.alt_pos);
 	
 	// Send a reply to the ground station
-	snprintf(buf, sizeof(buf), "Successfully set desired altitude to %.2f meters\r\n", structs->setpoint_struct.desiredQuadPosition.alt_pos);
-	unsigned char *responsePacket;
-
-	metadata_t metadata =
-	{
-		BEGIN_CHAR,
-		MessageTypes[5].ID,
-		MessageTypes[5].subtypes[1].ID,
-		0,
-		(strlen(buf) + 1)
-	};
-	formatPacket(&metadata, buf, &responsePacket);
-
-	// Send each byte of the packet individually
-	for(i = 0; i < 8 + metadata.data_len; i++) {
-		// Debug print statement for all of the bytes being sent
-		//printf("%d: 0x%x\n", i, responsePacket[i]);
-
-		uart0_sendByte(responsePacket[i]);
-	}
+	int length = snprintf(buf, sizeof(buf), "Successfully set desired altitude to %.2f meters\r\n", structs->setpoint_struct.desiredQuadPosition.alt_pos);
 
+	send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
 	return 0;
 }
 
@@ -778,7 +611,6 @@ int throttlep(unsigned char *packet, int dataLen,  modular_structs_t *structs)
 {
 	float value;
 	char buf[255] = {};
-	int i;
 	
 	memcpy(&value, ((float *)packet), dataLen);
 	structs->parameter_struct.alt_pid.Kp = value;
@@ -786,27 +618,9 @@ int throttlep(unsigned char *packet, int dataLen,  modular_structs_t *structs)
 	printf("function for throttlep: %f\n", structs->parameter_struct.alt_pid.Kp);
 	
 	// Send a reply to the ground station
-	snprintf(buf, sizeof(buf), "Successfully set alt Kp to %.2f\r\n", structs->parameter_struct.alt_pid.Kp);
-	unsigned char *responsePacket;
-
-	metadata_t metadata =
-	{
-		BEGIN_CHAR,
-		MessageTypes[5].ID,
-		MessageTypes[5].subtypes[1].ID,
-		0,
-		(strlen(buf) + 1)
-	};
-	formatPacket(&metadata, buf, &responsePacket);
-
-	// Send each byte of the packet individually
-	for(i = 0; i < 8 + metadata.data_len; i++) {
-		// Debug print statement for all of the bytes being sent
-		//printf("%d: 0x%x\n", i, responsePacket[i]);
-
-		uart0_sendByte(responsePacket[i]);
-	}
+	int length = snprintf(buf, sizeof(buf), "Successfully set alt Kp to %.2f\r\n", structs->parameter_struct.alt_pid.Kp);
 
+	send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
 	return 0;
 }
 
@@ -814,7 +628,6 @@ int throttlei(unsigned char *packet, int dataLen,  modular_structs_t *structs)
 {
 	float value;
 	char buf[255] = {};
-	int i;
 	
 	memcpy(&value, ((float *)packet), dataLen);
 	structs->parameter_struct.alt_pid.Ki = value;
@@ -822,27 +635,9 @@ int throttlei(unsigned char *packet, int dataLen,  modular_structs_t *structs)
 	printf("function for throttlei: %f\n", structs->parameter_struct.alt_pid.Ki);
 	
 	// Send a reply to the ground station
-	snprintf(buf, sizeof(buf), "Successfully set alt Ki to %.2f\r\n", structs->parameter_struct.alt_pid.Ki);
-	unsigned char *responsePacket;
-
-	metadata_t metadata =
-	{
-		BEGIN_CHAR,
-		MessageTypes[5].ID,
-		MessageTypes[5].subtypes[1].ID,
-		0,
-		(strlen(buf) + 1)
-	};
-	formatPacket(&metadata, buf, &responsePacket);
-
-	// Send each byte of the packet individually
-	for(i = 0; i < 8 + metadata.data_len; i++) {
-		// Debug print statement for all of the bytes being sent
-		//printf("%d: 0x%x\n", i, responsePacket[i]);
-
-		uart0_sendByte(responsePacket[i]);
-	}
+	int length = snprintf(buf, sizeof(buf), "Successfully set alt Ki to %.2f\r\n", structs->parameter_struct.alt_pid.Ki);
 
+	send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
 	return 0;
 }
 
@@ -850,7 +645,6 @@ int throttled(unsigned char *packet, int dataLen,  modular_structs_t *structs)
 {
 	float value;
 	char buf[255] = {};
-	int i;
 	
 	memcpy(&value, ((float *)packet), dataLen);
 	structs->parameter_struct.alt_pid.Kd = value;
@@ -858,27 +652,9 @@ int throttled(unsigned char *packet, int dataLen,  modular_structs_t *structs)
 	printf("function for throttled: %f\n", structs->parameter_struct.alt_pid.Kd);
 	
 	// Send a reply to the ground station
-	snprintf(buf, sizeof(buf), "Successfully set alt Kd to %.2f\r\n", structs->parameter_struct.alt_pid.Kd);
-	unsigned char *responsePacket;
-
-	metadata_t metadata =
-	{
-		BEGIN_CHAR,
-		MessageTypes[5].ID,
-		MessageTypes[5].subtypes[1].ID,
-		0,
-		(strlen(buf) + 1)
-	};
-	formatPacket(&metadata, buf, &responsePacket);
-
-	// Send each byte of the packet individually
-	for(i = 0; i < 8 + metadata.data_len; i++) {
-		// Debug print statement for all of the bytes being sent
-		//printf("%d: 0x%x\n", i, responsePacket[i]);
-
-		uart0_sendByte(responsePacket[i]);
-	}
+	int length = snprintf(buf, sizeof(buf), "Successfully set alt Kd to %.2f\r\n", structs->parameter_struct.alt_pid.Kd);
 
+	send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
 	return 0;
 }
 // These should be renamed to altitude!
diff --git a/quad/sw/modular_quad_pid/src/commands.h b/quad/sw/modular_quad_pid/src/commands.h
index 7861a84bd..cf234f0f8 100644
--- a/quad/sw/modular_quad_pid/src/commands.h
+++ b/quad/sw/modular_quad_pid/src/commands.h
@@ -5,6 +5,7 @@
 #include <stdlib.h>
 #include <string.h>
 #include "type_def.h"
+#include "packet_processing.h"
 
 // ----------------------
 // Helper stuff
diff --git a/quad/sw/modular_quad_pid/src/communication.c b/quad/sw/modular_quad_pid/src/communication.c
index 84eacfd72..73abd8537 100644
--- a/quad/sw/modular_quad_pid/src/communication.c
+++ b/quad/sw/modular_quad_pid/src/communication.c
@@ -1,177 +1,246 @@
 #include "communication.h"
 
-// QUAD & Ground Station
-// Format the log data from log_message
-//int formatData(unsigned char *log_msg, unsigned char *formattedCommand)
-int formatPacket(metadata_t *metadata, void *data, unsigned char **formattedCommand)
-{
-	*formattedCommand = malloc(sizeof(char) * metadata->data_len + 8);
-	/*if (formattedCommand == NULL)
-	{
+#define INTC		XScuGic
+#define COMM_UART_DEVICE_ID		XPAR_PS7_UART_0_DEVICE_ID
+#define COMM_INTC_DEVICE_ID		XPAR_SCUGIC_SINGLE_DEVICE_ID
+#define COMM_UART_INT_IRQ_ID		XPAR_PS7_UART_0_INTR//XPAR_XUARTPS_0_INTR
+
+#define BAUD_RATE 921600
+// Maximum number of bytes to be received within our loop time,
+// plus the maximum packet size that might be carried over from the last call,
+// plus 128 for a little buffer
+// (bit/s) * (seconds) / (10 bits / byte for UART)
+#define MAX_PACKET_SIZE 256
+#define UART_BUF_SIZE (((BAUD_RATE * (DESIRED_USEC_PER_LOOP / 1000) / 1000) / 10) + MAX_PACKET_SIZE + 128)
+
+// Declaration of interrupt handler
+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);
+	// Initialize UART0 (Bluetooth/WiFi)
+	if(!uartInstPtr) {
 		return -1;
-	}*/
-	
-	//----------------------------------------------------------------------------------------------
-	//	   index||	   0	|	  1	   |	  2		 |	3 & 4 |		 5 & 6		 |	7+	|	end	   |
-	//---------------------------------------------------------------------------------------------|
-	// msg param|| beg char | msg type | msg subtype | msg id | data len (bytes) | data | checksum |
-	//-------------------------------------------------------------------------------------------- |
-	//	   bytes||	   1	|	  1	   |	  1		 |	  2	  |		   2		 | var	|	 1	   |
-	//----------------------------------------------------------------------------------------------
-					
-	// Begin Char:
-	(*formattedCommand)[0] = metadata->begin_char;
-
-	// Msg type:
-	(*formattedCommand)[1] = metadata->msg_type;
-	
-	// Msg subtype
-	(*formattedCommand)[2] = metadata->msg_subtype;
-	
-	//Msg id (msgNum is 2 bytes)
-	(*formattedCommand)[3] = metadata->msg_id;
-	
-	// Data length and data - bytes 5&6 for len, 7+ for data
-	(*formattedCommand)[5] = metadata->data_len & 0x000000ff;
-	(*formattedCommand)[6] = (metadata->data_len >> 8) & 0x000000ff;
-	
-//	printf("data length %d\n", metadata->data_len);
-//	printf("data length %x\n", (*formattedCommand)[5]);
-//	printf("data length %x\n", (*formattedCommand)[6]);
-
-	memcpy(&((*formattedCommand)[7]), data, metadata->data_len);
-	
-	// Checksum
-	// receive data and calculate checksum
-	int i;
-	unsigned char packet_checksum = 0;
-	for(i = 0; i < 7 + metadata->data_len; i++)
-	{
-		packet_checksum ^= (*formattedCommand)[i];
 	}
-	
-//	printf("Packet checksum: 0x%02x\n", packet_checksum);
 
-	(*formattedCommand)[7 + metadata->data_len] = packet_checksum;
-	
-	return 0;
-}
+	uart0_clearFIFOs();
 
-// returns the length of the data in bytes (datalen from packet) and fills data
-// and metadata with the packet information
-// use as follows:
-//
-//		packet is the entire packet message (formatted) 
-//		data is an unallocated (char *) (pass it to this function as &data) 
-//		meta_data is a pointer to an instance of metadata_t
-//
-int parse_packet(unsigned char * packet, unsigned char ** data, metadata_t * meta_data)
-{
-	//----------------------------------------------------------------------------------------------
-	//     index||     0    |     1    |      2      |  3 & 4 |      5 & 6       |  7+  |   end    |
-	//---------------------------------------------------------------------------------------------|
-	// msg param|| beg char | msg type | msg subtype | msg id | data len (bytes) | data | checksum |
-	//-------------------------------------------------------------------------------------------- |
-	//     bytes||     1    |     1    |      1      |    2   |        2         | var  |    1     |
-	//----------------------------------------------------------------------------------------------	
-	
-	// first byte must be the begin char
-	if(packet[0] != 0xBE) {
-		printf("The first packet byte is not the begin char.\n");
+	if (uart0_int_init(COMM_UART_INT_IRQ_ID, (Xil_ExceptionHandler) uart_interrupt_handler) != XST_SUCCESS) {
 		return -1;
 	}
 
-	// receive 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];
-
-//	printf("msg_type: %x\n", meta_data->msg_type);
-//	printf("msg_subtype: %x\n", meta_data->msg_subtype);
-//	printf("msg_type: %d\n", meta_data->data_len);
-	
-	int i;
-	
-	// receive data
-	*data = malloc(meta_data->data_len);
-	for(i = 0; i < meta_data->data_len; i++)
-	{
-		(*data)[i] = packet[7+i];
-	}
 
-	// calculate checksum
+	/*
+	 * 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, 4);
+
+	// Second argument is the number of bytes to trigger an interrupt at
+	XUartPs_SetFifoThreshold(uartInstPtr, 48);
+
+	return 0;
+}
+
+int process_packet(unsigned char* 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);
+
+	// Compute checksum
+	int i;
 	unsigned char calculated_checksum = 0;
-	for(i = 0; i < meta_data->data_len + 7; i++)
-	{
+	for(i = 0; i < meta_data.data_len + 7; i++){
 		calculated_checksum ^= packet[i];
 	}
-
-	// compare checksum
+	// Discard if checksum didn't match
 	if(packet_checksum != calculated_checksum) {
-		return 1;
-		printf("Checksums did not match (Quadlog): 0x%02x\t0x%02x\n", packet_checksum, calculated_checksum);
+		printf("Checksums did not match: 0x%02x\t0x%02x\n", packet_checksum, calculated_checksum);
+		return -1;
 	}
 
-	//////////////////////////////
-	// Send an acknowledgment packet
+	// Call appropriate function for packet
+	(* (MessageTypes[meta_data.msg_type].subtypes[meta_data.msg_subtype].functionPtr))(packet_data, meta_data.data_len, structs);
 
-	// Send a reply to the ground station
-/*
-	int buf = meta_data->msg_id;
-	unsigned char *responsePacket;
-
-	metadata_t metadata =
-	{
-		BEGIN_CHAR,
-		MessageTypes[0].ID,
-		MessageTypes[0].subtypes[1].ID,
-		0,
-		sizeof(int)
-	};
-	formatPacket(&metadata, &buf, &responsePacket);
-
-	// Send each byte of the packet individually
-	for(i = 0; i < 8 + metadata.data_len; i++) {
-		// Debug print statement for all of the bytes being sent
-		//printf("%d: 0x%x\n", i, responsePacket[i]);
-
-		uart0_sendByte(responsePacket[i]);
-	}
-	free(responsePacket);
-*/
 	return 0;
+
 }
 
-// QUAD & Ground Station
-// Process the command received
-int processCommand(unsigned char *packet, modular_structs_t *structs) {
-	int validPacket;
-	unsigned char *data;
-	metadata_t metadata;
-	
-	printf("Process Command.\n");
-
-	// Validate the message is correctly formatted
-	validPacket = parse_packet(packet, &data, &metadata);
-	if(validPacket != 0) {
-		printf("Packet is not valid.\n");
-		return -1;
+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;
+		}
+	}
+}
+
+/*
+ * 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);
+}
+
+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);
+	//unsigned char in_fifo = XUartPs_ReadReg(uartInstPtr->Config.BaseAddress, XUARTPS_FIFO_OFFSET);
+	return;
+}
+
+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);
+
+	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;
+		CsrRegister = XUartPs_ReadReg(InstancePtr->Config.BaseAddress, XUARTPS_SR_OFFSET);
 	}
-	
-	if(metadata.data_len >= 0) {
-		// Call the appropriate subtype function
-		(* (MessageTypes[metadata.msg_type].subtypes[metadata.msg_subtype].functionPtr))(data, metadata.data_len, structs);
-		
-//		printf("%s\n", MessageTypes[metadata.msg_type].subtypes[metadata.msg_subtype].cmdText);
-
-		return 0;
-	} else {
-		printf("Data length is less than 0.\n");
+
+	// Clear the interrupt status.
+	XUartPs_WriteReg(InstancePtr->Config.BaseAddress, XUARTPS_ISR_OFFSET,
+			IsrStatus);
+}
+
+int send_data(u16 type_id, u16 subtype_id, u16 msg_id, char* data, size_t size) {
+	//----------------------------------------------------------------------------------------------
+	//	   index||	   0	|	  1	   |	  2		 |	3 & 4 |		 5 & 6		 |	7+	|	end	   |
+	//---------------------------------------------------------------------------------------------|
+	// msg param|| beg char | msg type | msg subtype | msg id | data len (bytes) | data | checksum |
+	//-------------------------------------------------------------------------------------------- |
+	//	   bytes||	   1	|	  1	   |	  1		 |	  2	  |		   2		 | var	|	 1	   |
+	//----------------------------------------------------------------------------------------------
+
+	char formattedHeader[7];
+
+	// Begin Char:
+	formattedHeader[0] = BEGIN_CHAR;
+	// Msg type:
+	formattedHeader[1] = type_id;
+	// Msg subtype
+	formattedHeader[2] = subtype_id;
+	//Msg id 2 bytes
+	formattedHeader[3] = msg_id & 0x000000ff;
+	formattedHeader[4] = (msg_id >> 8) & 0x000000ff;
+	// Data length and data - bytes 5&6 for len, 7+ for data
+	formattedHeader[5] = size & 0x000000ff;
+	formattedHeader[6] = (size >> 8) & 0x000000ff;
+
+	// Compute checksum while sending
+	unsigned char packet_checksum = 0;
+
+	//int i;
+	// TODO: Look into uart0_sendBytes and see if it would be better to use
+	// Send header
+	uart0_sendBytes(formattedHeader, 7);
+	/*
+	for(i = 0; i < 7; i++) {
+		packet_checksum ^= formattedHeader[i];
+		uart0_sendByte(formattedHeader[i]);
 	}
-	
-	// Only gets here if there is an error
-	return -1;
+	// Send data
+	for (i = 0; i < size; i++) {
+		packet_checksum ^= data[i];
+		uart0_sendByte(data[i]);
+	}*/
+	// Send data
+	uart0_sendBytes(data, size);
+	// Send checksum
+	uart0_sendByte(packet_checksum);
+
+	return 0;
 }
diff --git a/quad/sw/modular_quad_pid/src/communication.h b/quad/sw/modular_quad_pid/src/communication.h
index 8778c4edf..df87802cc 100644
--- a/quad/sw/modular_quad_pid/src/communication.h
+++ b/quad/sw/modular_quad_pid/src/communication.h
@@ -1,18 +1,20 @@
 #ifndef _COMMUNICATION_H
 #define _COMMUNICATION_H
 
-#include <stdio.h>
-#include <string.h>
-#include <stdlib.h>
-#include <limits.h>
-#include "commands.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"
 
-int formatCommand(unsigned char *command, unsigned char **formattedCommand);
-int formatPacket(metadata_t *metadata, void *data, unsigned char **formattedCommand);
-int logData(unsigned char *log_msg, unsigned char *formattedCommand);
-int processCommand(unsigned char *command, modular_structs_t *structs);
-int parse_packet(unsigned char * packet, unsigned char ** data, metadata_t * meta_data);
+int initUartComms();
+void process_received(modular_structs_t *structs);
+void uart_interrupt_handler(XUartPs *InstancePtr);
+int send_data(u16 type_id, u16 subtype_id, u16 msg_id, char* data, size_t size);
 
 #endif
diff --git a/quad/sw/modular_quad_pid/src/control_algorithm.c b/quad/sw/modular_quad_pid/src/control_algorithm.c
index b057a30be..2e79f58fb 100644
--- a/quad/sw/modular_quad_pid/src/control_algorithm.c
+++ b/quad/sw/modular_quad_pid/src/control_algorithm.c
@@ -92,15 +92,9 @@
 		log_struct->trims.throttle = sensor_struct->trims.throttle;
 	}
 
-	if(user_input_struct->hasPacket == 0x04 && user_defined_struct->engaging_auto == 1)
+	if(user_input_struct->locationFresh && user_defined_struct->engaging_auto == 1)
 			user_defined_struct->engaging_auto = 2;
 
-	// If the quad has received a packet and it's not an update packet
-	if(user_input_struct->hasPacket != -1 && user_input_struct->hasPacket != 0x04)
-	{
-		processCommand((unsigned char *)user_input_struct->sb->buf, structs);
-	}
-
 
 	// if the flap switch was toggled to AUTO_FLIGHT_MODE and we've received a new packet
 	// then record the current position as the desired position
@@ -139,7 +133,7 @@
 
 //		static int counter_between_packets = 0;
 
-	if(user_input_struct->hasPacket == 0x04)
+	if(user_input_struct->locationFresh)
 	{
 		parameter_struct->local_y_pid.current_point = sensor_struct->currentQuadPosition.y_pos;
 		parameter_struct->local_y_pid.setpoint = setpoint_struct->desiredQuadPosition.y_pos;
@@ -334,11 +328,8 @@
 
 	last_fm_switch = cur_fm_switch;
 
-	if(user_input_struct->hasPacket != -1)
-	{
-		user_input_struct->sb->clear(user_input_struct->sb);
-		user_input_struct->hasPacket = -1;
-	}
+	// Make location stale now
+	user_input_struct->locationFresh = 0;
 
     return 0;
  }
diff --git a/quad/sw/modular_quad_pid/src/initialize_components.c b/quad/sw/modular_quad_pid/src/initialize_components.c
index 9e8322e2b..324bf1a3c 100644
--- a/quad/sw/modular_quad_pid/src/initialize_components.c
+++ b/quad/sw/modular_quad_pid/src/initialize_components.c
@@ -10,7 +10,7 @@
 
 extern int Xil_AssertWait;
 
-int protection_loops()
+int protection_loops(modular_structs_t *structs)
 {
 	int rc_commands[6]; // 6 "receiver_input" elements: Throttle, Pitch, Roll, Yaw, Gear, and Flap
 
@@ -26,51 +26,13 @@ int protection_loops()
 		read_rec_all(rc_commands);
 
 	// wait until the ground station has connected to the quad and acknowledged that its ready to start
-	stringBuilder_t * ack_packet = stringBuilder_create();
-	while(1)
-	{
-		// --------------------------------------
-		// Send request to ground station to start sending VRPN
-		char buf[255] = {};
-		int i;
-
-		// Debug print statement
-		//printf("function for yawset: %f\n", structs->setpoint_struct.desiredQuadPosition.yaw);
-
-		// Send a reply to the ground station
-		snprintf(buf, sizeof(buf), "The quad is ready to receive VRPN data.\r\n");
-		unsigned char *responsePacket;
-
-		metadata_t metadata =
-		{
-			BEGIN_CHAR,
-			MessageTypes[4].ID,
-			MessageTypes[4].subtypes[1].ID,
-			0,
-			(strlen(buf) + 1)
-		};
-		formatPacket(&metadata, buf, &responsePacket);
-
-		// Send each byte of the packet individually
-		for(i = 0; i < 8 + metadata.data_len; i++) {
-			// Debug print statement for all of the bytes being sent
-			printf("%d: 0x%x\n", i, responsePacket[i]);
-
-			uart0_sendByte(responsePacket[i]);
-		}
+	char buf[255] = {};
+	int length = snprintf(buf, sizeof(buf), "The quad is ready to receive VRPN data.\r\n");
+	length = length >= sizeof(buf) ? 255 : length;
 
+	while (!structs->user_input_struct.receivedBeginUpdate) {
+		send_data(MessageTypes[4].ID, MessageTypes[4].subtypes[1].ID, 0, buf, length);
 		usleep(10000);
-
-		tryReceivePacket(ack_packet, 0);
-
-		unsigned char * data;
-		metadata_t md;
-		parse_packet((unsigned char *) ack_packet->buf, &data, &md);
-
-		if(md.msg_type == 0x04 && md.msg_subtype == 0x01)
-			break;
-
-		// --------------------------------------
 	}
 
 	// let the pilot/observers know that the quad is now active
@@ -86,28 +48,19 @@ int initializeAllComponents(user_input_t * user_input_struct, log_t * log_struct
     // Turn off LED 7 to let observers know that the quad is not yet active
 	MIO7_led_off();
 
-	// Use the stringbuilder to keep track of data received
-	if(!(user_input_struct->sb = stringBuilder_create()))
-		return -1;
-
 	// Initialize the controller
 	control_algorithm_init(parameter_struct);
 
 	// Xilinx given initialization
 	init_platform();
 
-	//disable blocking asserts
-	//Xil_AssertWait = FALSE;
-
 	// Initialize UART0 (Bluetooth)
-	if(!uart0_init(XPAR_PS7_UART_0_DEVICE_ID, 921600))
+	if (initUartComms()) {
 		return -1;
+	}
 
 	uart0_clearFIFOs();
 
-	//Enable blocking asserts
-	//Xil_AssertWait = TRUE;
-
 	// Initialize I2C controller and start the sensor board
 	if (initI2C0() == -1) {
 		return -1;
@@ -117,7 +70,9 @@ int initializeAllComponents(user_input_t * user_input_struct, log_t * log_struct
 	pwm_init();
 
 	// Initialize loop timers
-	timer_init();
+	if (timer_init()) {
+		return -1;
+	}
 
 	//manual flight mode
 	user_defined_struct->flight_mode = MANUAL_FLIGHT_MODE;
diff --git a/quad/sw/modular_quad_pid/src/initialize_components.h b/quad/sw/modular_quad_pid/src/initialize_components.h
index 3c5b95e37..8a1c66e75 100644
--- a/quad/sw/modular_quad_pid/src/initialize_components.h
+++ b/quad/sw/modular_quad_pid/src/initialize_components.h
@@ -24,7 +24,7 @@
  *      error message
  *
  */
-int protection_loops();
+int protection_loops(modular_structs_t *structs);
 
 /**
  * @brief
diff --git a/quad/sw/modular_quad_pid/src/log_data.c b/quad/sw/modular_quad_pid/src/log_data.c
index 2acac6dc7..f1e9e7a82 100644
--- a/quad/sw/modular_quad_pid/src/log_data.c
+++ b/quad/sw/modular_quad_pid/src/log_data.c
@@ -147,28 +147,7 @@ void printLogging(){
 	strcat(buf,header);
 	strcat(buf,units);
 
-	// Send a reply to the ground station
-	unsigned char *responsePacket;
-
-	metadata_t metadata =
-	{
-		BEGIN_CHAR,
-		MessageTypes[5].ID,
-		MessageTypes[5].subtypes[0].ID,
-		0,
-		(strlen(buf) + 1)
-	};
-	formatPacket(&metadata, buf, &responsePacket);
-
-//	printf("Checksum: 0x%02x", responsePacket[metadata.data_len + 7]);
-
-	// Send each byte of the packet individually
-	for(i = 0; i < 8 + metadata.data_len; i++) {
-		uart0_sendByte(responsePacket[i]);
-		if(i < 8 || i == metadata.data_len + 8)
-			printf("%d: 0x%02x\n", i, responsePacket[i]);
-	}
-
+	send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[0].ID, 0, buf, strlen(buf) + 1);
 	//uart0_sendBytes(buf, strlen(buf));
 	//usleep(100000);
 
@@ -176,36 +155,8 @@ void printLogging(){
 	/* print & send log data */
 	for(i = 0; i < arrayIndex; i++){
 		char* logLine = format(logArray[i]);
-
-
-		metadata_t metadata =
-		{
-			BEGIN_CHAR,
-			MessageTypes[5].ID,
-			MessageTypes[5].subtypes[0].ID,
-			0,
-			(strlen(logLine) + 1)
-		};
-		formatPacket(&metadata, (u8 *)logLine, &responsePacket);
-
-		// Send each byte of the packet individually
-//		for(j = 0; j < 8 + metadata.data_len; j++) {
-//			uart0_sendByte(responsePacket[j]);
-//			printf("%d: 0x%02x\n", j, responsePacket[j]);
-//		}
-//		uart0_sendBytes(responsePacket, 8 + metadata.data_len);
-
-		uart0_sendMetaData(metadata);
-		uart0_sendBytes(logLine, metadata.data_len);
-		//uart0_sendByte(0);
-		uart0_sendByte(responsePacket[7 + metadata.data_len]);
-
-//
-//		if((i % 5) == 0)
-		usleep(15000);
-
+		send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[0].ID, 0, logLine, strlen(logLine) + 1);
 		free(logLine);
-		//break;
 	}
 }
 
diff --git a/quad/sw/modular_quad_pid/src/main.c b/quad/sw/modular_quad_pid/src/main.c
index dac9246ca..7595fc878 100644
--- a/quad/sw/modular_quad_pid/src/main.c
+++ b/quad/sw/modular_quad_pid/src/main.c
@@ -35,7 +35,7 @@ int main()
 	}
 
 	// Loops to make sure the quad is responding correctly before starting the control loop
-	protection_loops();
+	protection_loops(&structs);
 
 	printf("The quad loop is now beginning.\n");
 
@@ -96,9 +96,6 @@ int main()
 
 	} while(!kill_condition(&(structs.user_input_struct)));
 
-
-	stringBuilder_free((structs.user_input_struct).sb);
-
 	pwm_kill();
 
 	MIO7_led_off();
diff --git a/quad/sw/modular_quad_pid/src/packet_processing.c b/quad/sw/modular_quad_pid/src/packet_processing.c
index 65f44dc53..c6764a00b 100644
--- a/quad/sw/modular_quad_pid/src/packet_processing.c
+++ b/quad/sw/modular_quad_pid/src/packet_processing.c
@@ -33,43 +33,6 @@ tokenList_t tokenize(char* cmd) {
 	return ret;
 }
 
-int processUpdate(unsigned char* update, quadPosition_t* currentQuadPosition) {
-	//static char buf[16384];
-	//sprintf(buf, "update..(%d)..[%s]\r\n", strlen(update), update);
-	//uart0_sendStr(buf);
-
-	unsigned char * data;
-	metadata_t md;
-	parse_packet(update, &data, &md);
-
-
-	// Packet must come as [NEARPY], 4 bytes each
-	int packetId = getInt(data, 0);
-//	printf("Packet ID: %d\n", packetId);
-	float y_pos = getFloat(data, 4);
-//	printf("y_pos: %f\n", y_pos);
-	float x_pos = getFloat(data, 8);
-//	printf("x_pos: %f\n", x_pos);
-	float alt_pos = getFloat(data, 12);
-//	printf("alt_pos: %f\n", alt_pos);
-	float roll = getFloat(data, 16);
-//	printf("roll: %f\n", roll);
-	float pitch = getFloat(data, 20);
-//	printf("pitch: %f\n", pitch);
-	float yaw = getFloat(data, 24);
-//	printf("yaw: %f\n", yaw);
-
-	currentQuadPosition->packetId = packetId;
-	currentQuadPosition->y_pos = y_pos;
-	currentQuadPosition->x_pos = x_pos;
-	currentQuadPosition->alt_pos = alt_pos;
-	currentQuadPosition->roll = roll;
-	currentQuadPosition->pitch = pitch;
-	currentQuadPosition->yaw = yaw;
-
-	return 0;
-}
-
 float getFloat(unsigned char* str, int pos) {
 	union {
 		float f;
diff --git a/quad/sw/modular_quad_pid/src/sensor.c b/quad/sw/modular_quad_pid/src/sensor.c
index b2f60c86d..86d1aebd4 100644
--- a/quad/sw/modular_quad_pid/src/sensor.c
+++ b/quad/sw/modular_quad_pid/src/sensor.c
@@ -26,11 +26,6 @@ int sensor_init(raw_sensor_t * raw_sensor_struct, sensor_t * sensor_struct)
 
 int get_sensors(log_t* log_struct, user_input_t* user_input_struct, raw_sensor_t* raw_sensor_struct)
 {
-	// if there was a new update packet this loop then do the required processing
-	if (user_input_struct->hasPacket == 0x04) {
-		processUpdate((unsigned char *) user_input_struct->sb->buf, &(raw_sensor_struct->currentQuadPosition));
-	}
-
 //	///////// for testing update period of vrpn data from the ground station to the quad
 //	static int update_counter = 0;
 //	if(user_input_struct->hasPacket == 0x04)
diff --git a/quad/sw/modular_quad_pid/src/stringBuilder.c b/quad/sw/modular_quad_pid/src/stringBuilder.c
deleted file mode 100644
index d830cfb02..000000000
--- a/quad/sw/modular_quad_pid/src/stringBuilder.c
+++ /dev/null
@@ -1,199 +0,0 @@
-/*
- * stringBuilder.c
- *
- *  Created on: Sep 24, 2014
- *      Author: ucart
- */
-
-#include <stdlib.h>
-#include <string.h>
-#include "stringBuilder.h"
-
-int stringBuilder_maybeExpand(stringBuilder_t* sb) {
-	if (!sb || !sb->buf) {
-		return STRINGBUILDER_ILLEGALARGUMENT;
-	}
-
-	// See if we need to expand
-	if (sb->length + 1 >= sb->capacity) {
-		if (sb->capacity >= sb->maxCapacity) {
-			// Would exceed maxCapacity, can't do anything
-			return STRINGBUILDER_AT_MAX_CAPACITY;
-		}
-
-		// Compute new size (try doubling)
-		int newCapacity = sb->capacity * 2;
-		if (newCapacity <= 2) {
-			newCapacity = 2;
-		}
-		if (newCapacity >= sb->maxCapacity) {
-			newCapacity = sb->maxCapacity;
-		}
-
-		// Get a pointer to the old buf mem
-		char* oldBuf = sb->buf;
-		sb->buf = malloc(newCapacity);
-		if (!sb->buf) {
-			// Agh, no mem for buf. Restore the old one and return error
-			sb->buf = oldBuf;
-			return STRINGBUILDER_NO_MEM_FOR_EXPANSION;
-		} else {
-			// Got mem for new buf, copy from old buf
-			//strncpy(sb->buf, oldBuf, sb->length + 1);
-			memcpy(sb->buf, oldBuf, sb->length);
-			sb->capacity = newCapacity;
-			free(oldBuf);
-		}
-	}
-
-	return STRINGBUILDER_SUCCESS;
-}
-
-stringBuilder_t* stringBuilder_createWithMaxCapacity(int initialCapacity,
-		int maxCapacity) {
-	// Invalid arguments
-	if (initialCapacity > maxCapacity || initialCapacity < 1
-			|| maxCapacity < 1) {
-		return NULL ;
-	}
-
-	stringBuilder_t* sb = (stringBuilder_t*) malloc(sizeof(stringBuilder_t));
-	if (!sb) {
-		// No mem for buffer
-		return NULL ;
-	}
-
-	// Try to allocate mem for buf
-	sb->buf = malloc(initialCapacity);
-	if (!sb->buf) {
-		// No mem for buf
-		free(sb);
-		return NULL ;
-	}
-
-	// Set up function pointers
-	sb->addStr = stringBuilder_addStr;
-	sb->addStrAt = stringBuilder_addStrAt;
-	sb->addChar = stringBuilder_addChar;
-	sb->addCharAt = stringBuilder_addCharAt;
-	sb->removeCharAt = stringBuilder_removeCharAt;
-	sb->clear = stringBuilder_clear;
-
-
-	// Good to go
-	sb->length = 0;
-	sb->capacity = initialCapacity;
-	sb->maxCapacity = maxCapacity;
-	sb->buf[0] = '\0';
-	return sb;
-}
-
-stringBuilder_t* stringBuilder_createWithInitialCapacity(int initialCapacity) {
-	return stringBuilder_createWithMaxCapacity(initialCapacity,
-			STRINGBUILDER_DEFAULT_MAX_CAPACITY);
-}
-
-stringBuilder_t* stringBuilder_create() {
-	return stringBuilder_createWithMaxCapacity(
-			STRINGBUILDER_DEFAULT_INITIAL_CAPACITY,
-			STRINGBUILDER_DEFAULT_MAX_CAPACITY);
-}
-
-void stringBuilder_free(stringBuilder_t* sb) {
-	if (sb && sb->buf) {
-		free(sb->buf);
-	}
-	if (sb) {
-		free(sb);
-	}
-}
-
-int stringBuilder_addStr(stringBuilder_t* sb, char* str) {
-	if (!sb || !sb->buf) {
-		return STRINGBUILDER_ILLEGALARGUMENT;
-	}
-
-	while (*str) {
-		int status = sb->addChar(sb, *str);
-		if (status != STRINGBUILDER_SUCCESS) {
-			return status;
-		}
-		str++;
-	}
-
-	return STRINGBUILDER_SUCCESS;
-}
-
-int stringBuilder_addStrAt(stringBuilder_t* sb, char* str, int index) {
-	if (!sb || !sb->buf) {
-		return STRINGBUILDER_ILLEGALARGUMENT;
-	}
-
-	while (*str) {
-		int status = sb->addCharAt(sb, *str, index);
-		if (status != STRINGBUILDER_SUCCESS) {
-			return status;
-		}
-
-		str++;
-		index++;
-	}
-
-	return STRINGBUILDER_SUCCESS;
-}
-
-/** Add a character to end of the StringBuilder */
-int stringBuilder_addChar(stringBuilder_t* sb, char c) {
-	if (!sb || !sb->buf) {
-		return STRINGBUILDER_ILLEGALARGUMENT;
-	}
-
-	return stringBuilder_addCharAt(sb, c, sb->length);
-}
-
-/** Add a character to the StringBuilder at the specified index, if possible */
-int stringBuilder_addCharAt(stringBuilder_t* sb, char c, int index) {
-	if (!sb || !sb->buf || index < 0 || index > sb->length) {
-		return STRINGBUILDER_ILLEGALARGUMENT;
-	}
-
-	// Try expanding if necessary
-	int status = stringBuilder_maybeExpand(sb);
-	if (status != STRINGBUILDER_SUCCESS) {
-		return status;
-	}
-
-	// Move everything right of index over by 1
-	int i;
-	for (i = sb->length; i >= index; i--) {
-		sb->buf[i + 1] = sb->buf[i];
-	}
-
-	// Insert the character and add the null terminator
-	sb->buf[index] = c;
-	sb->length += 1;
-
-	return STRINGBUILDER_SUCCESS;
-}
-
-/** Remove a character from the StringBuilder at the specified index */
-int stringBuilder_removeCharAt(stringBuilder_t* sb, int index) {
-	if (!sb || !sb->buf || index < 0 || index >= sb->length) {
-		return STRINGBUILDER_ILLEGALARGUMENT;
-	}
-
-	// Move everything right of index over left
-	int i;
-	for (i = index; i < sb->length; i++) {
-		sb->buf[i] = sb->buf[i + 1];
-	}
-	sb->length -= 1;
-
-	return STRINGBUILDER_SUCCESS;
-}
-
-void stringBuilder_clear(stringBuilder_t* sb) {
-	sb->length = 0;
-	sb->buf[0] = '\0';
-}
-
diff --git a/quad/sw/modular_quad_pid/src/stringBuilder.h b/quad/sw/modular_quad_pid/src/stringBuilder.h
deleted file mode 100644
index efcfe6276..000000000
--- a/quad/sw/modular_quad_pid/src/stringBuilder.h
+++ /dev/null
@@ -1,59 +0,0 @@
-/*
- * stringBuffer.h
- *
- *  Created on: Sep 24, 2014
- *      Author: ucart
- */
-
-#ifndef STRINGBUILDER_H_
-#define STRINGBUILDER_H_
-
-#include "type_def.h"
-
-#define STRINGBUILDER_DEFAULT_INITIAL_CAPACITY 16
-#define STRINGBUILDER_DEFAULT_MAX_CAPACITY 1024
-
-enum {
-	STRINGBUILDER_SUCCESS = 0,
-	STRINGBUILDER_AT_MAX_CAPACITY = 1,
-	STRINGBUILDER_NO_MEM_FOR_EXPANSION = 2,
-	STRINGBUILDER_ILLEGALARGUMENT = 3
-};
-
-/***** Constructors *****/
-
-/** Create a StringBuilder with a max/initial capacity specified */
-stringBuilder_t* stringBuilder_createWithMaxCapacity(int, int);
-
-/** Create a StringBuilder with an initial capacity specified */
-stringBuilder_t* stringBuilder_createWithInitialCapacity(int);
-
-/** Create a StringBuilder with default initial capacity/max capacity */
-stringBuilder_t* stringBuilder_create();
-
-
-/** Free a StringBuilder */
-void stringBuilder_free(stringBuilder_t*);
-
-
-/***** Methods *****/
-
-/** Add a string to the end of the StringBuilder */
-int stringBuilder_addStr(stringBuilder_t*, char*);
-
-/** Add a string to the StringBuilder at the specified index */
-int stringBuilder_addStrAt(stringBuilder_t*, char*, int);
-
-/** Add a character to end of the StringBuilder */
-int stringBuilder_addChar(stringBuilder_t*, char);
-
-/** Add a character to the StringBuilder at the specified index */
-int stringBuilder_addCharAt(stringBuilder_t*, char, int);
-
-/** Remove a character from the StringBuilder at the specified index */
-int stringBuilder_removeCharAt(stringBuilder_t*, int);
-
-/** Clear a stringBuilder */
-void stringBuilder_clear(stringBuilder_t*);
-
-#endif /* STRINGBUILDER_H_ */
diff --git a/quad/sw/modular_quad_pid/src/timer.c b/quad/sw/modular_quad_pid/src/timer.c
index 8c99522c5..7d5f3b11b 100644
--- a/quad/sw/modular_quad_pid/src/timer.c
+++ b/quad/sw/modular_quad_pid/src/timer.c
@@ -18,9 +18,7 @@ int timer_init()
 {
 
 	// using a axi_timer core because we've had problems with the Global Timer
-	XTmrCtr_Initialize(&axi_timer, XPAR_AXI_TIMER_0_DEVICE_ID);
-
-	return 0;
+	return XTmrCtr_Initialize(&axi_timer, XPAR_AXI_TIMER_0_DEVICE_ID);
 }
 
 int timer_start_loop()
diff --git a/quad/sw/modular_quad_pid/src/type_def.h b/quad/sw/modular_quad_pid/src/type_def.h
index 55dca9e8f..a13879a48 100644
--- a/quad/sw/modular_quad_pid/src/type_def.h
+++ b/quad/sw/modular_quad_pid/src/type_def.h
@@ -8,6 +8,7 @@
 #ifndef TYPE_DEF_H_
 #define TYPE_DEF_H_
 
+#include <stdint.h>
 /**
  * @brief
  *      The modes for autonomous and manual flight.
@@ -29,27 +30,11 @@ typedef struct {
 	char begin_char;
 	char msg_type;
 	char msg_subtype;
-	int msg_id;
-	int data_len;
+	uint16_t msg_id;
+	uint16_t data_len;
 } metadata_t;
 
 
-// String builder data type
-typedef struct stringBuilder_s {
-	char* buf;
-	int length;
-	int capacity;
-	int maxCapacity;
-
-	// Methods
-	int (*addStr)(struct stringBuilder_s*, char*);
-	int (*addStrAt)(struct stringBuilder_s*, char*, int);
-	int (*addChar)(struct stringBuilder_s*, char);
-	int (*addCharAt)(struct stringBuilder_s*, char, int);
-	int (*removeCharAt)(struct stringBuilder_s*, int);
-	void (*clear)(struct stringBuilder_s*);
-} stringBuilder_t;
-
 typedef struct {
 	char** tokens;
 	int numTokens;
@@ -163,8 +148,8 @@ typedef struct user_input_t {
 	float roll_angle_manual_setpoint;
 	float pitch_angle_manual_setpoint;
 
-	int hasPacket;
-	stringBuilder_t * sb;
+	int locationFresh;
+	int receivedBeginUpdate;
 } user_input_t;
 
 /**
diff --git a/quad/sw/modular_quad_pid/src/uart.c b/quad/sw/modular_quad_pid/src/uart.c
index c45aa1c97..d4bf4ed44 100644
--- a/quad/sw/modular_quad_pid/src/uart.c
+++ b/quad/sw/modular_quad_pid/src/uart.c
@@ -16,6 +16,7 @@
 // Global PS's
 XUartPs* _Uart0PS;
 XUartPs* _Uart1PS;
+static INTC Intc;
 
 //This is copied from xuart driver
 /***************************************************/
@@ -194,6 +195,71 @@ char uart_recvByte(XUartPs* uartps_ptr) {
 //	return buffer[0];
 }
 
+
+/*****************************************************************************/
+/**
+*
+* This function sets up the interrupt system so interrupts can occur for the
+* Uart. This function is application-specific.
+*
+* @param	IntcInstancePtr is a pointer to the instance of the INTC.
+* @param	UartInstancePtr contains a pointer to the instance of the UART
+*		driver which is going to be connected to the interrupt
+*		controller.
+* @param	UartIntrId is the interrupt Id and is typically
+*		XPAR_<UARTPS_instance>_INTR value from xparameters.h.
+*
+* @return	XST_SUCCESS if successful, otherwise XST_FAILURE.
+*
+* @note		None.
+*
+****************************************************************************/
+int SetupInterruptSystem(XUartPs *UartInstancePtr, u16 UartIntrId, Xil_ExceptionHandler handler)
+{
+	int Status;
+
+	XScuGic_Config *IntcConfig; /* Config for interrupt controller */
+
+	/* Initialize the interrupt controller driver */
+	IntcConfig = XScuGic_LookupConfig(INTC_DEVICE_ID);
+	if (NULL == IntcConfig) {
+		return XST_FAILURE;
+	}
+
+	Status = XScuGic_CfgInitialize(&Intc, IntcConfig,
+					IntcConfig->CpuBaseAddress);
+	if (Status != XST_SUCCESS) {
+		return XST_FAILURE;
+	}
+
+	/*
+	 * Connect the interrupt controller interrupt handler to the
+	 * hardware interrupt handling logic in the processor.
+	 */
+	Xil_ExceptionRegisterHandler(XIL_EXCEPTION_ID_INT,
+				(Xil_ExceptionHandler) XScuGic_InterruptHandler,
+				&Intc);
+
+	/*
+	 * Connect a device driver handler that will be called when an
+	 * interrupt for the device occurs, the device driver handler
+	 * performs the specific interrupt processing for the device
+	 */
+	Status = XScuGic_Connect(&Intc, UartIntrId,
+				  handler,
+				  (void *) UartInstancePtr);
+	if (Status != XST_SUCCESS) {
+		return XST_FAILURE;
+	}
+
+	/* Enable the interrupt for the device */
+	XScuGic_Enable(&Intc, UartIntrId);
+
+	/* Enable interrupts */
+	 Xil_ExceptionEnable();
+
+	return XST_SUCCESS;
+}
 /************************************************/
 /************************************************/
 
@@ -212,6 +278,11 @@ XUartPs* uart0_init(u16 deviceID, int baudRate){
 	return uart_init(_Uart0PS, deviceID, baudRate);
 }
 
+// Initializes the interrupt system for UART 0
+int uart0_int_init(u16 UartIntrId, Xil_ExceptionHandler handler) {
+	return SetupInterruptSystem(_Uart0PS, UartIntrId, handler);
+}
+
 void uart0_clearFIFOs(){
 	uart_clearFIFOs(_Uart0PS);
 }
diff --git a/quad/sw/modular_quad_pid/src/uart.h b/quad/sw/modular_quad_pid/src/uart.h
index 50d468962..6629e21af 100644
--- a/quad/sw/modular_quad_pid/src/uart.h
+++ b/quad/sw/modular_quad_pid/src/uart.h
@@ -10,12 +10,16 @@
 
 #include "xparameters.h"
 #include "xuartps.h"
-#include "stringBuilder.h"
+#include "xscugic.h"
+#include "communication.h"
 
 #define PACKET_START_CHAR 2
 #define PACKET_END_CHAR 3
 #define UPDATE_SIZE 28
 
+#define INTC		XScuGic // Interrupt controller type
+#define INTC_DEVICE_ID		XPAR_SCUGIC_SINGLE_DEVICE_ID
+
 extern XUartPs* _Uart0PS;
 extern XUartPs* _Uart1PS;
 
@@ -47,6 +51,7 @@ char uart_recvByte(XUartPs* uartps_ptr);
 /************************************************/
 /********** UART 0 convenience methods **********/
 XUartPs* uart0_init(u16 deviceID, int baudRate);
+int uart0_int_init(u16 UartIntrId, Xil_ExceptionHandler handler);
 void uart0_clearFIFOs();
 void uart0_sendByte(u8 data);
 void uart0_sendStr(char* data);
@@ -73,8 +78,5 @@ int uart1_hasData();
 void uart1_recvBytes(char* buffer, int numBytes);
 char uart1_recvByte();
 /************************************************/
-/************************************************/
-
-int tryReceivePacket(stringBuilder_t* sb, int echo);
 
 #endif /* UART_H_ */
diff --git a/quad/sw/modular_quad_pid/src/user_input.h b/quad/sw/modular_quad_pid/src/user_input.h
index 282e15dbe..c17fe91fe 100644
--- a/quad/sw/modular_quad_pid/src/user_input.h
+++ b/quad/sw/modular_quad_pid/src/user_input.h
@@ -12,7 +12,6 @@
 #include "type_def.h"
 #include "log_data.h"
 #include "util.h"
-#include "stringBuilder.h"
 
 /*
  * Aero channel declaration
-- 
GitLab