diff --git a/controls/MATLAB/momentOfInertia/J_calc_bifilar.m b/controls/MATLAB/momentOfInertia/J_calc_bifilar.m
index 9f52a69ec50752c0c2b2b644f1a5b373ccc74bcb..0e7945bfff9acae2a5dfcefe2c7f0278ccbca208 100644
--- a/controls/MATLAB/momentOfInertia/J_calc_bifilar.m
+++ b/controls/MATLAB/momentOfInertia/J_calc_bifilar.m
@@ -1,6 +1,14 @@
 function  J  = J_calc_bifilar( m , g, D, L, t, n , varargin)
-%UNTITLED Summary of this function goes here
-%   Detailed explanation goes here
+% J  = J_calc_bifilar( m , g, D, L, t, n)
+% J  = J_calc_bifilar( m , g, D, L, t, n , mode_option)
+% 
+% m , D , L , t , and n are vectors containing the mass, distance between
+% strings, legnth of strings, time, and cycles of a set of tests. g is the
+% acceleration of gravity you want to use. 
+% 
+% mode_option is an optional input which will specify how to calculate J: 
+% mode_option = 0 will use averageing of the trials (default) 
+% mode_option = 1 will use least squares of the whole data set 
 
 average_strings = {'average','mean','ave'}; 
 least_squares_strings = {'least squares','ls'}; 
@@ -30,7 +38,5 @@ if mode == 1 %least squares
     J = (16*pi^2*L./(m*g.*D.^2))\(t./n).^2 ; 
 end
 
-
-
 end
 
diff --git a/quad/Makefile b/quad/Makefile
index 0a92c686ac1fd60dc693dbc0a8f0c86f0768331a..8f71d2be8b8812670837040173bfdd7d823271e2 100644
--- a/quad/Makefile
+++ b/quad/Makefile
@@ -38,8 +38,8 @@ test: all
 	$(MAKE) -C src/queue test
 	$(MAKE) -C src/computation_graph test
 	$(MAKE) -C src/quad_app test
-	ruby scripts/tests/test_safety_checks.rb
 	ruby scripts/tests/test_memory_integrity.rb
+	ruby scripts/tests/test_safety_checks.rb
 	ruby scripts/tests/test_communication.rb
 
 clean:
diff --git a/quad/doc/zybo_pinout_assignments.md b/quad/doc/zybo_pinout_assignments.md
index 05c3de7e849595b35d19101580e31fed3ad2b52f..c2bdae857218bdbae5dc933b1553161554ce533a 100644
--- a/quad/doc/zybo_pinout_assignments.md
+++ b/quad/doc/zybo_pinout_assignments.md
@@ -8,10 +8,10 @@ The Zybo board has 6 PMOD Connectors. We use these for most quad I/O.
 ```
 Zybo PMOD Pin : Zync Pin : Xilinx Pin Name : Our Assignment
 
-JF1  : MIO-13 : N/A : unused
+JF1  : MIO-13 : N/A : I2C1 SDA
 JF2  : MIO-10 : N/A : I2C0 SCL
 JF3  : MIO-11 : N/A : I2C0 SDA
-JF4  : MIO-12 : N/A : unused
+JF4  : MIO-12 : N/A : I2C1 SCL
 JF7  : MIO-0  : N/A : unused
 JF8  : MIO-9  : N/A : unused
 JF9  : MIO-14 : N/A : unused
@@ -40,8 +40,8 @@ JC2  : W15 : IO_L10N_T1_34 : processing_system7_0_UART0_TX_pin
 JC3  : T11 : IO_L1P_T0_34  : processing_system7_0_UART0_RX_pin
 JC4  : T10 : IO_L1N_T0_34  : unused
 JC7  : W14 : IO_L8P_T1_34  : unused
-JC8  : Y14 : IO_L8N_T1_34  : unused
-JC9  : T12 : IO_L2P_T0_34  : unused
+JC8  : Y14 : IO_L8N_T1_34  : processing_system7_0_UART1_TX_pin
+JC9  : T12 : IO_L2P_T0_34  : processing_system7_0_UART1_RX_pin
 JC10 : U12 : IO_L2N_T0_34  : unused
 
 JB1  : T20 : IO_L15P_T2_DQS_34 : unused
diff --git a/quad/scripts/tests/test_memory_integrity.rb b/quad/scripts/tests/test_memory_integrity.rb
index adb8f9190b6adb3fff8a6e6f8de2eb143854cfe3..87cbc83b8795758275fdc63d682857f7bdd4f97d 100644
--- a/quad/scripts/tests/test_memory_integrity.rb
+++ b/quad/scripts/tests/test_memory_integrity.rb
@@ -14,7 +14,7 @@ Dir.chdir(bin_dir)
 puts("Firing up the quad...")
 
 # Start virtual quad
-quad = Process.spawn("valgrind --leak-check=full --log-file=./valgrind.out ./virt-quad start")
+quad = Process.spawn("valgrind -v --leak-check=full --log-file=./valgrind.out ./virt-quad start")
 
 sleep 1.5
 
diff --git a/quad/src/quad_app/callbacks.c b/quad/src/quad_app/callbacks.c
index cb99d83ad9d108e297121666df94159e96fbaedd..8c06e3bdd5283a234578398a2246a1a322291ba5 100644
--- a/quad/src/quad_app/callbacks.c
+++ b/quad/src/quad_app/callbacks.c
@@ -28,7 +28,7 @@ int cb_debug(struct modular_structs *structs, struct metadata *meta, unsigned ch
 	float param_val = graph_get_output(graph, node_id, 0);
 
 	int len = snprintf((char*)buf, sizeof buf, "%f", param_val);
-	send_data(&structs->hardware_struct.uart, DEBUG_ID, 0, buf, len >= sizeof(buf) ? 255 : length + 1);
+	send_data(structs->hardware_struct.comm.uart, DEBUG_ID, 0, buf, len >= sizeof(buf) ? 255 : length + 1);
 	return 0;
 }
 
@@ -40,7 +40,7 @@ int cb_packetlog(struct modular_structs* structs, struct metadata *meta, u8 *dat
 	n_msg_received += 1;
 	total_payload_received += length;
 	int len = sprintf(buf, "Packets received: %d", n_msg_received);
-	send_data(&structs->hardware_struct.uart, PACKETLOG_ID, 0, buf, len);
+	send_data(structs->hardware_struct.comm.uart, PACKETLOG_ID, 0, buf, len);
 	return 0;
 }
 
@@ -54,7 +54,7 @@ int cb_getpacketlogs(struct modular_structs* structs, struct metadata *meta, u8
 	// Message logging number of messages received and size of payload received
 	int len = snprintf((char*)buf, sizeof buf, "%d,%lu", n_msg_received, total_payload_received);
 
-	send_data(&structs->hardware_struct.uart, LOG_ID, 0, buf, len >= sizeof(buf) ? 255 : len + 1);
+	send_data(structs->hardware_struct.comm.uart, LOG_ID, 0, buf, len >= sizeof(buf) ? 255 : len + 1);
 	return 0;
 }
 
@@ -202,7 +202,7 @@ int cb_getparam(struct modular_structs *structs, struct metadata *meta, unsigned
 	pack_float(param_val, resp_data + 4);
 
 	// Send the response
-	send_data(&structs->hardware_struct.uart, RESPPARAM_ID, msg_id, resp_data, sizeof(resp_data));
+	send_data(structs->hardware_struct.comm.uart, RESPPARAM_ID, msg_id, resp_data, sizeof(resp_data));
 
 	return 5;
 }
@@ -274,7 +274,7 @@ int cb_getsource(struct modular_structs *structs, struct metadata *meta, unsigne
 	pack_short(source.controller_id, resp_data + 4);
 	pack_short(source.controller_output, resp_data + 6);
 
-	send_data(&structs->hardware_struct.uart, RESPSOURCE_ID, msg_id, resp_data, sizeof(resp_data));
+	send_data(structs->hardware_struct.comm.uart, RESPSOURCE_ID, msg_id, resp_data, sizeof(resp_data));
 	return 0;
 }
 
@@ -303,7 +303,7 @@ int cb_getoutput(struct modular_structs *structs, struct metadata *meta, unsigne
 	pack_float(output_val, resp_data + 4);
 
 	// Send the response
-	send_data(&structs->hardware_struct.uart, RESPOUTPUT_ID, msg_id, resp_data, sizeof(resp_data));
+	send_data(structs->hardware_struct.comm.uart, RESPOUTPUT_ID, msg_id, resp_data, sizeof(resp_data));
 
 	return 0;
 }
@@ -325,7 +325,7 @@ int cb_getnodes(struct modular_structs *structs, struct metadata *meta, unsigned
 	const struct computation_graph* graph = structs->parameter_struct.graph;
 	if (graph->n_nodes >= 150) {
 		static char* error_msg = "Over 150 nodes. Not responding to cb_getnodes for fear of buffer overflow.";
-		send_data(&structs->hardware_struct.uart, DEBUG_ID, 0,
+		send_data(structs->hardware_struct.comm.uart, DEBUG_ID, 0,
 				(u8*)error_msg, sizeof(error_msg));
 		return -1;
 	}
@@ -369,7 +369,7 @@ int cb_getnodes(struct modular_structs *structs, struct metadata *meta, unsigned
 		}
 	}
 
-	send_data(&structs->hardware_struct.uart, RESPNODES_ID, meta->msg_id, resp_buf, offset);
+	send_data(structs->hardware_struct.comm.uart, RESPNODES_ID, meta->msg_id, resp_buf, offset);
 	return 0;
 }
 
@@ -406,6 +406,6 @@ int cb_addnode(struct modular_structs *structs, struct metadata *meta, unsigned
 	u8 resp_buf[2];
 	pack_short(new_node_id, resp_buf);
 
-	send_data(&structs->hardware_struct.uart, RESPADDNODE_ID, meta->msg_id, resp_buf, sizeof(resp_buf));
+	send_data(structs->hardware_struct.comm.uart, RESPADDNODE_ID, meta->msg_id, resp_buf, sizeof(resp_buf));
 	return 0;
 }
diff --git a/quad/src/quad_app/communication.c b/quad/src/quad_app/communication.c
index aa7561a20a2b389a0a21e39cfc585192ce8a61cc..2f0183c984d3ad1d38ff8a07d3f72a8427675481 100644
--- a/quad/src/quad_app/communication.c
+++ b/quad/src/quad_app/communication.c
@@ -96,7 +96,7 @@ int process_packet(modular_structs_t *structs) {
 
 void process_received(modular_structs_t *structs) {
   // Parse as many packets as possible
-  struct UARTDriver *uart = &structs->hardware_struct.uart;
+  struct UARTDriver *uart = structs->hardware_struct.comm.uart;
   while (!try_receive_packet(uart)) {
     process_packet(structs);
   }
@@ -111,33 +111,34 @@ int send_data(struct UARTDriver *uart, u16 type_id, u16 msg_id, u8* data, size_t
 	//	   bytes||	   1	|	       2             |   2    |        2         | var	|	 1	   |
 	//----------------------------------------------------------------------------------------------
 
-	unsigned char formattedHeader[PACKET_HEADER_SIZE];
-
-	// Begin Char:
-	formattedHeader[0] = BEGIN_CHAR;
-	// Msg type 2 bytes:
-	formattedHeader[1] = type_id & 0x000000ff;
-	formattedHeader[2] = (type_id >> 8) & 0x000000ff;
-	// 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
-	unsigned char packet_checksum = 0;
-	int i;
-	for(i = 0; i < PACKET_HEADER_SIZE; i++) {
-		packet_checksum ^= formattedHeader[i];
-		uart->write(uart, formattedHeader[i]);
-	}
-	for (i = 0; i < size; i++) {
-		packet_checksum ^= data[i];
-		uart->write(uart, data[i]);
-	}
-
-	uart->write(uart, packet_checksum);
-
-	return 0;
+  
+  unsigned char formattedHeader[PACKET_HEADER_SIZE];
+
+  // Begin Char:
+  formattedHeader[0] = BEGIN_CHAR;
+  // Msg type 2 bytes:
+  formattedHeader[1] = type_id & 0x000000ff;
+  formattedHeader[2] = (type_id >> 8) & 0x000000ff;
+  // 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
+  unsigned char packet_checksum = 0;
+  int i;
+  for(i = 0; i < PACKET_HEADER_SIZE; i++) {
+    packet_checksum ^= formattedHeader[i];
+    uart->write(uart, formattedHeader[i]);
+  }
+  for (i = 0; i < size; i++) {
+    packet_checksum ^= data[i];
+    uart->write(uart, data[i]);
+  }
+
+  uart->write(uart, packet_checksum);
+
+  return 0;
 }
diff --git a/quad/src/quad_app/hw_iface.h b/quad/src/quad_app/hw_iface.h
index 1a5f5109ce012bc531d73cbcfe40761e0613882e..03aa86e66da8e7f6f2f152b33602cc99280346f9 100644
--- a/quad/src/quad_app/hw_iface.h
+++ b/quad/src/quad_app/hw_iface.h
@@ -1,8 +1,6 @@
 #ifndef HW_IFACE_H
 #define HW_IFACE_H
 
-#include "xbasic_types.h"
-
 /**
  * Hardware Interfaces
  *
@@ -26,6 +24,7 @@
 struct gam;
 struct lidar;
 struct px4flow;
+struct gps;
 
 struct RCReceiverDriver {
   void *state;
@@ -68,7 +67,6 @@ struct SystemDriver {
 
 struct I2CDriver {
   void *state;
-  int busId;
   int (*reset)(struct I2CDriver *self);
   int (*write)(struct I2CDriver *self,
                unsigned short device_addr,
@@ -98,4 +96,14 @@ struct OpticalFlowDriver {
   int (*read)(struct OpticalFlowDriver *self, struct px4flow *of);
 };
 
+struct GPSDriver {
+  struct UARTDriver *uart;
+  int (*reset)(struct GPSDriver *self, struct gps *gps);
+  int (*read)(struct GPSDriver *self, struct gps *gps);
+};
+
+struct CommDriver {
+  struct UARTDriver *uart;
+};
+
 #endif
diff --git a/quad/src/quad_app/initialize_components.c b/quad/src/quad_app/initialize_components.c
index b39bf638ff782b148457e012759c667567beee0a..edccbbbf88d06f4d48bc7bd24053eeb8d8dd43dc 100644
--- a/quad/src/quad_app/initialize_components.c
+++ b/quad/src/quad_app/initialize_components.c
@@ -64,7 +64,7 @@ int init_structs(modular_structs_t *structs) {
   timer_init_globals(global_timer, axi_timer);
 
   // Initialize UART0
-  struct UARTDriver *uart = &structs->hardware_struct.uart;
+  struct UARTDriver *uart = &structs->hardware_struct.uart_0;
   if (uart->reset(uart)) {
     return -1;
   }
diff --git a/quad/src/quad_app/log_data.c b/quad/src/quad_app/log_data.c
index cd724d2b2ba0681c3b36ba53932b0c8c78a26af5..2a29ddb8f798a1e9a80dfe6c879e196358621f67 100644
--- a/quad/src/quad_app/log_data.c
+++ b/quad/src/quad_app/log_data.c
@@ -186,7 +186,7 @@ int log_data(log_t* log_struct, parameter_t* ps)
  * TODO: This should probably be transmitting in binary instead of ascii
  */
 
-void printLogging(hardware_t *hardware_struct, log_t* log_struct, parameter_t* ps, raw_sensor_t* raw_sensors){
+void printLogging(struct CommDriver *comm, log_t* log_struct, parameter_t* ps, raw_sensor_t* raw_sensors){
 	if (arrayIndex == 0) {
 		// Don't send any logs if nothing was logged
 		return;
@@ -197,7 +197,7 @@ void printLogging(hardware_t *hardware_struct, log_t* log_struct, parameter_t* p
 	// Let user know that allocation failed
 	if (arraySize != LOG_STARTING_SIZE) {
 		safe_sprintf_cat(&buf, "Failed to allocate enough log memory\n");
-		send_data(&hardware_struct->uart, LOG_ID, 0, (u8*)buf.str, buf.size);
+		send_data(comm->uart, LOG_ID, 0, (u8*)buf.str, buf.size);
 		return;
 	}
 
@@ -240,7 +240,7 @@ void printLogging(hardware_t *hardware_struct, log_t* log_struct, parameter_t* p
 	safe_sprintf_cat(&buf, "\n");
 
 	// Send header names
-	send_data(&hardware_struct->uart, LOG_ID, 0, (u8*)buf.str, buf.size);
+	send_data(comm->uart, LOG_ID, 0, (u8*)buf.str, buf.size);
 
 	// Send units header
 	buf.size = 0;
@@ -248,17 +248,17 @@ void printLogging(hardware_t *hardware_struct, log_t* log_struct, parameter_t* p
 	safe_sprintf_cat(&buf, units_output.str);
 	safe_sprintf_cat(&buf, units_param.str);
 	safe_sprintf_cat(&buf, "\n");
-	send_data(&hardware_struct->uart, LOG_ID, 0, (u8*)buf.str, buf.size);
+	send_data(comm->uart, LOG_ID, 0, (u8*)buf.str, buf.size);
 
 	/*************************/
 	/* print & send log data */
 	for(i = 0; i < arrayIndex; i++){
 		format_log(i, log_struct, &buf);
-		send_data(&hardware_struct->uart, LOG_ID, 0, (u8*)buf.str, buf.size);
+		send_data(comm->uart, LOG_ID, 0, (u8*)buf.str, buf.size);
 	}
 
 	// Empty message of type LOG_END to indicate end of log
-	send_data(&hardware_struct->uart, LOG_END_ID, 0, (u8*)buf.str, 0);
+	send_data(comm->uart, LOG_END_ID, 0, (u8*)buf.str, 0);
 }
 
 void resetLogging() {
diff --git a/quad/src/quad_app/log_data.h b/quad/src/quad_app/log_data.h
index c3df9ec19c0e9b6cc41517bc4990b22db44add5c..9c4b8b7d9cf61f9d86bca21543100b5c48fdb38f 100644
--- a/quad/src/quad_app/log_data.h
+++ b/quad/src/quad_app/log_data.h
@@ -47,7 +47,7 @@ void addParamToLog(log_t* log_struct, int controller_id, int param_id, char* uni
  /**
   * Prints all the log information.
   */
- void printLogging(hardware_t *hardware_struct, log_t* log_struct, parameter_t* ps, raw_sensor_t* raw_sensors);
+ void printLogging(struct CommDriver *comm, log_t* log_struct, parameter_t* ps, raw_sensor_t* raw_sensors);
 
  /**
   * Resets and clears logged data
diff --git a/quad/src/quad_app/quad_app.c b/quad/src/quad_app/quad_app.c
index 7f7db6b4fbf3734ea357bc6214bd53c8411c16bd..561df5b1a26cb6394d487751cab9aeef5c227b7a 100644
--- a/quad/src/quad_app/quad_app.c
+++ b/quad/src/quad_app/quad_app.c
@@ -100,7 +100,7 @@ int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
 
 		if (this_kill_condition == 1 && last_kill_condition == 0) {
 		  // Just disabled
-		  printLogging(&structs.hardware_struct, &(structs.log_struct), &(structs.parameter_struct), &structs.raw_sensor_struct);
+		  printLogging(&structs.hardware_struct.comm, &(structs.log_struct), &(structs.parameter_struct), &structs.raw_sensor_struct);
 		  resetLogging();
 		  MIO7_led_off();
 		}
@@ -113,8 +113,8 @@ int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
 		if (structs.raw_sensor_struct.gam.error.consErrorCount > 10) {
 			kill_motors(&(structs.hardware_struct.motors));
 			char err_msg[] = "More than 10 IMU errors";
-			send_data(&structs.hardware_struct.uart, DEBUG_ID, 0, (u8*)err_msg, sizeof(err_msg));
-			printLogging(&structs.hardware_struct, &(structs.log_struct), &(structs.parameter_struct), &structs.raw_sensor_struct);
+			send_data(structs.hardware_struct.comm.uart, DEBUG_ID, 0, (u8*)err_msg, sizeof(err_msg));
+			printLogging(&structs.hardware_struct.comm, &(structs.log_struct), &(structs.parameter_struct), &structs.raw_sensor_struct);
 			break;
 		}
 	}
diff --git a/quad/src/quad_app/sensor_processing.c b/quad/src/quad_app/sensor_processing.c
index 80e2c22d38f100d47ae5fe2560ede1602da18305..ad1ee5e5e2da5679fa732d5fdf61ab895656ffcc 100644
--- a/quad/src/quad_app/sensor_processing.c
+++ b/quad/src/quad_app/sensor_processing.c
@@ -125,14 +125,14 @@ int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_se
 	/*
 	 * Altitude double complementary filter
 	 */
-	static float alt_alpha = 0.9975;
+	static float alt_alpha = 0.98;
 	static float filtered_vel = 0;
 	static float filtered_alt = 0;
 	static float last_lidar = 0;
 	
-	float this_lidar = raw_sensor_struct->lidar_distance_m;
+	float this_lidar = -raw_sensor_struct->lidar_distance_m;
 	// Acceleration in m/s without gravity
-	float quad_z_accel = -9.8 * (accel_z + 1);
+	float quad_z_accel = 9.8 * (accel_z + 1);
 	filtered_vel = alt_alpha*(filtered_vel + quad_z_accel*get_last_loop_time()) +
 	              (1 - alt_alpha)*(this_lidar - last_lidar);
 	filtered_alt = alt_alpha*(filtered_alt + filtered_vel*get_last_loop_time()) +
diff --git a/quad/src/quad_app/type_def.h b/quad/src/quad_app/type_def.h
index ccf8a677cfe88abb4bb4705ff0240a7cce6fce4c..d5bd1d45e240dd71e06ac784fc4c75e8b4e406d0 100644
--- a/quad/src/quad_app/type_def.h
+++ b/quad/src/quad_app/type_def.h
@@ -131,6 +131,14 @@ typedef struct px4flow {
 	SensorError_t error;
 } px4flow_t;
 
+typedef struct gps {
+	double lat, lon;
+	double vel, course;
+
+	int fixed;
+	unsigned int numSats;
+} gps_t;
+
 typedef struct PID_t {
 	float current_point;	// Current value of the system
 	float setpoint;		// Desired value of the system
@@ -454,7 +462,10 @@ typedef struct hardware_t {
   struct I2CDriver i2c_1;
   struct RCReceiverDriver rc_receiver;
   struct MotorDriver motors;
-  struct UARTDriver uart;
+  struct UARTDriver uart_0;
+  struct UARTDriver uart_1;
+  struct GPSDriver gps;
+  struct CommDriver comm;
   struct TimerDriver global_timer;
   struct TimerDriver axi_timer;
   struct LEDDriver mio7_led;
diff --git a/quad/src/virt_quad/hw_impl_unix.c b/quad/src/virt_quad/hw_impl_unix.c
index d3607f17e16deffee36a4cefe64a6b0a55f15943..ab1704ca719e1358a46bc099d5cc7c2e06b7e495 100644
--- a/quad/src/virt_quad/hw_impl_unix.c
+++ b/quad/src/virt_quad/hw_impl_unix.c
@@ -9,6 +9,12 @@ struct UARTDriver create_unix_uart() {
   return uart;
 }
 
+struct CommDriver create_unix_comm(struct UARTDriver *uart) {
+  struct CommDriver comm;
+  comm.uart = uart;
+  return comm;
+}
+
 struct MotorDriver create_unix_motors() {
   struct MotorDriver motors;
   motors.state = NULL;
diff --git a/quad/src/virt_quad/hw_impl_unix.h b/quad/src/virt_quad/hw_impl_unix.h
index b368f3fb50630324524ec9f50b1bb8daceb1287d..3ec22683d223584cb4865bc8f58051c733d7e4b5 100644
--- a/quad/src/virt_quad/hw_impl_unix.h
+++ b/quad/src/virt_quad/hw_impl_unix.h
@@ -78,6 +78,7 @@ int unix_optical_flow_reset(struct OpticalFlowDriver *self, struct px4flow *of);
 int unix_optical_flow_read(struct OpticalFlowDriver *self, struct px4flow *of);
 
 struct UARTDriver create_unix_uart();
+struct CommDriver create_unix_comm(struct UARTDriver *uart);
 struct MotorDriver create_unix_motors();
 struct RCReceiverDriver create_unix_rc_receiver();
 struct I2CDriver create_unix_i2c();
diff --git a/quad/src/virt_quad/main.c b/quad/src/virt_quad/main.c
index 1e24ab458cec3ca535a21a7a538d51c4b08f6eb3..0d9620db899f86192d978200677f16ad89978025 100644
--- a/quad/src/virt_quad/main.c
+++ b/quad/src/virt_quad/main.c
@@ -31,17 +31,19 @@ void help_sub_command(char *argv[]);
  * Implement each of the hardware interfaces.
  */
 int setup_hardware(hardware_t *hardware) {
-  hardware->i2c = create_unix_i2c();
+  hardware->i2c_0 = create_unix_i2c();
+  hardware->i2c_1 = create_unix_i2c();
   hardware->rc_receiver = create_unix_rc_receiver();
   hardware->motors = create_unix_motors();
-  hardware->uart = create_unix_uart();
+  hardware->uart_0 = create_unix_uart();
+  hardware->comm = create_unix_comm(&hardware->uart_0);
   hardware->global_timer = create_unix_global_timer();
   hardware->axi_timer = create_unix_axi_timer();
   hardware->mio7_led = create_unix_mio7_led();
   hardware->sys = create_unix_system();
-  hardware->imu = create_unix_imu(&hardware->i2c);
-  hardware->lidar = create_unix_lidar(&hardware->i2c);
-  hardware->of = create_unix_optical_flow(&hardware->i2c);
+  hardware->imu = create_unix_imu(&hardware->i2c_0);
+  hardware->lidar = create_unix_lidar(&hardware->i2c_1);
+  hardware->of = create_unix_optical_flow(&hardware->i2c_0);
   return 0;
 }
 
@@ -121,6 +123,7 @@ void open_new_shm_io() {
   pthread_mutex_init(&virt_quad_io->led_lock, &attr);
   pthread_mutex_init(&virt_quad_io->motors_lock, &attr);
   pthread_mutex_init(&virt_quad_io->rc_lock, &attr);
+  pthread_mutex_init(&virt_quad_io->i2c_lock, &attr);
 }
 
 void open_existing_shm_io() {
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.c
index 78cf4aafd4f291db173f595de9b0a826f6a929c7..df4cd13a3180ba7fe76c125cc8a72ce89e9aaa8c 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.c
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.c
@@ -1,14 +1,33 @@
 #include "hw_impl_zybo.h"
 
-struct UARTDriver create_zybo_uart() {
+struct UARTDriver create_zybo_uart(int devId) {
   struct UARTDriver uart;
-  uart.state = NULL;
+  uart.state = malloc(sizeof(struct ZyboUARTState));
+  struct ZyboUARTState *state = uart.state;
+  state->inst = malloc(sizeof(XUartPs));
+  state->queue = queue_malloc(MAX_UART_BUFFER_SIZE);
+  state->devId = devId;
+
   uart.reset = zybo_uart_reset;
   uart.write = zybo_uart_write;
   uart.read = zybo_uart_read;
   return uart;
 }
 
+struct GPSDriver create_zybo_gps(struct UARTDriver *uart) {
+  struct GPSDriver gps;
+  gps.uart = uart;
+  gps.reset = zybo_gps_reset;
+  gps.read = zybo_gps_read;
+  return gps;
+}
+
+struct CommDriver create_zybo_comm(struct UARTDriver *uart) {
+  struct CommDriver comm;
+  comm.uart = uart;
+  return comm;
+}
+
 struct MotorDriver create_zybo_motors() {
   struct MotorDriver motors;
   motors.state = NULL;
@@ -27,8 +46,10 @@ struct RCReceiverDriver create_zybo_rc_receiver() {
 
 struct I2CDriver create_zybo_i2c(int busId) {
   struct I2CDriver i2c;
-  i2c.busId = busId;
-  i2c.state = NULL;
+  i2c.state = malloc(sizeof(struct ZyboI2CState));
+  struct ZyboI2CState *state = i2c.state;
+  state->inst = malloc(sizeof(XIicPs));
+  state->busId = busId;
   i2c.reset = zybo_i2c_reset;
   i2c.write = zybo_i2c_write;
   i2c.read = zybo_i2c_read;
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.h b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.h
index 2f577ee3f7f6bdb60211d20d4c7a78c6c0381a5a..8f439cdf1872f1da5bcd0638d76a152f58c27f8f 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.h
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.h
@@ -24,6 +24,20 @@
 #define PX4FLOW_DEVICE_ADDR			0x42
 #define PX4FLOW_QUAL_MIN			(100)
 
+#define MAX_UART_BUFFER_SIZE 2048
+
+struct ZyboI2CState {
+  XIicPs *inst;
+  int busId;
+};
+
+struct ZyboUARTState {
+  struct Queue *queue;
+  XUartPs *inst;
+  XScuGic xscugic;
+  int devId;
+};
+
 int zybo_uart_reset(struct UARTDriver *self);
 int zybo_uart_write(struct UARTDriver *self, unsigned char c);
 int zybo_uart_read(struct UARTDriver *self, unsigned char *c);
@@ -68,7 +82,12 @@ int zybo_lidar_read(struct LidarDriver *self, struct lidar *lidar);
 int zybo_optical_flow_reset(struct OpticalFlowDriver *self, struct px4flow *of);
 int zybo_optical_flow_read(struct OpticalFlowDriver *self, struct px4flow *of);
 
-struct UARTDriver create_zybo_uart();
+int zybo_gps_reset(struct GPSDriver *self, gps_t *gps);
+int zybo_gps_read(struct GPSDriver *self, gps_t *gps);
+
+struct UARTDriver create_zybo_uart(int devId);
+struct CommDriver create_zybo_comm(struct UARTDriver *uart);
+struct GPSDriver create_zybo_gps(struct UARTDriver *uart);
 struct MotorDriver create_zybo_motors();
 struct RCReceiverDriver create_zybo_rc_receiver();
 struct I2CDriver create_zybo_i2c(int busId);
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_gps.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_gps.c
new file mode 100644
index 0000000000000000000000000000000000000000..ff495afdbc2a597e3e2ca9ba89343cefeee6ed09
--- /dev/null
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_gps.c
@@ -0,0 +1,12 @@
+#include "hw_iface.h"
+#include "hw_impl_zybo.h"
+
+int zybo_gps_reset(struct GPSDriver *self, gps_t *gps) {
+  // TODO
+  return -1;
+}
+
+int zybo_gps_read(struct GPSDriver *self, gps_t *gps) {
+  // TODO
+  return -1;
+}
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_i2c.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_i2c.c
index 27f85b241f652d95f2e47c7d95a9f7708e1439b6..751ae4ccf5bbc741def8cea9ea0e555be8f230f0 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_i2c.c
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_i2c.c
@@ -43,15 +43,13 @@ int XIicPs_MasterRecvPolled_ours(XIicPs *InstancePtr, u8 *MsgPtr,
 int XIicPs_SetupMaster(XIicPs *InstancePtr, int Role);
 
 int zybo_i2c_reset(struct I2CDriver *self) {
-  int i2cID = self->busId;
-  if(i2cID > 1) {
-	  return -1;
-  }
-
-  if (self->state == NULL) {
-    self->state = malloc(sizeof(XIicPs));
-  }
-  XIicPs *inst = self->state;
+  // ensure all required memory is allocated
+  struct ZyboI2CState *state = self->state;
+  if (state == NULL) return -1;
+  if (state->inst == NULL) return -1;
+  if (state->busId > 1) return -1;
+  int i2cID = state->busId;
+  XIicPs *inst = state->inst;
 
   //Make sure CPU_1x clk is enabled fostatusr I2C controller
   u16 *aper_ctrl = (u16 *) IO_CLK_CONTROL_REG_ADDR;
@@ -86,7 +84,8 @@ int zybo_i2c_write(struct I2CDriver *self,
                    unsigned short device_addr,
                    unsigned char *data,
                    unsigned int length) {
-  XIicPs *inst = self->state;
+  struct ZyboI2CState *state = self->state;
+  XIicPs *inst = state->inst;
   if (device_addr == PX4FLOW_DEVICE_ADDR) {
 	  // If we are sending a request to optical flow, drop down to 100kHz
 	  XIicPs_SetSClk(inst, 100000);
@@ -104,7 +103,8 @@ int zybo_i2c_read(struct I2CDriver *self,
                   unsigned short device_addr,
                   unsigned char *buff,
                   unsigned int length) {
-  XIicPs *inst = self->state;
+  struct ZyboI2CState *state = self->state;
+  XIicPs *inst = state->inst;
   if (device_addr == PX4FLOW_DEVICE_ADDR) {
 	  // If we are sending a request to optical flow, drop down to 100kHz
 	  XIicPs_SetSClk(inst, 100000);
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_imu.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_imu.c
index 2efebf246ee42886f1d8dc6f3fd8073ee1307ed5..da9ed7ffc6346f652d13391ae6acfd738171892b 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_imu.c
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_imu.c
@@ -57,8 +57,7 @@ int mpu9150_read_mag(struct IMUDriver *self, gam_t* gam);
 int mpu9150_read_gyro_accel(gam_t* gam);
 
 int zybo_imu_reset(struct IMUDriver *self, gam_t *gam) {
-  // TODO: initialize gam
-	 memset(gam, 0, sizeof(gam_t));
+  memset(gam, 0, sizeof(gam_t));
 
   struct I2CDriver *i2c = self->i2c;
 
@@ -143,7 +142,7 @@ int mpu9150_write(struct I2CDriver *i2c, u8 register_addr, u8 data){
 
   // Check if within register range
   if(register_addr < 0 || register_addr > 0x75){
-    return;
+    return -1;
   }
 
   if(register_addr <= 0x12){
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_tests.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_tests.c
index 9c02bdb6338734c41c8afe352682366739410f86..a38e87306620ba54d308d61cd450cf7e817614cc 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_tests.c
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_tests.c
@@ -65,9 +65,9 @@ int test_zybo_i2c() {
 int test_zybo_i2c_imu() {
   struct I2CDriver i2c = create_zybo_i2c(0);
   struct IMUDriver imu = create_zybo_imu(&i2c);
-  i2c.reset(&i2c);
 
   gam_t gam;
+  if (i2c.reset(&i2c)) return 0;
   if (imu.reset(&imu, &gam)) return 0;
 
   int status = 0;
@@ -280,7 +280,7 @@ int test_zybo_global_timer() {
 }
 
 /**
- * Test for the UARTDriver.
+ * Communication tests using the UARTDriver.
  *
  * Instructions:
  * 1) Connect Zybo board to computer by USB cable.
@@ -296,15 +296,16 @@ int test_zybo_global_timer() {
  *    - Observe test results on terminal
  *    - You might be able to see LED MIO7 blink when it receives bytes
  */
-int test_zybo_uart() {
-  struct UARTDriver uart = create_zybo_uart();
+int test_zybo_uart_comm() {
+  struct UARTDriver uart = create_zybo_uart(0);
+  struct CommDriver comm = create_zybo_comm(&uart);
   struct LEDDriver led = create_zybo_mio7_led();
   uart.reset(&uart);
   led.reset(&led);
 
   unsigned char c;
   while (1) {
-    if (uart.read(&uart, &c)) {
+    if (comm.uart->read(comm.uart, &c)) {
       // read failed
       led.turn_off(&led);
     } else {
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_uart.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_uart.c
index 18c17af0a045f70686b001c0de488eacd6efbeaf..9d3ff70509cdd8837a2278f4e721d6340bb3d77b 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_uart.c
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_uart.c
@@ -1,29 +1,20 @@
 #include "hw_impl_zybo.h"
 
 #define BAUD_RATE 921600
-#define MAX_UART_BUFFER_SIZE 2048
 
-int XUartPs_SetBaudRate_ours(XUartPs *InstancePtr, u32 BaudRate);
-void uart_interrupt_handler(XUartPs *InstancePtr);
-int SetupInterruptSystem(XUartPs *UartInstancePtr, u16 UartIntrId, Xil_ExceptionHandler handler);
-void uart_interrupt_handler(XUartPs *InstancePtr);
 
-// Queue has to be global to be accessible to ISR
-static struct Queue queue;
-static volatile unsigned char buff[MAX_UART_BUFFER_SIZE];
-static XScuGic xscugic;
+int XUartPs_SetBaudRate_ours(XUartPs *InstancePtr, u32 BaudRate);
+int SetupInterruptSystem(struct ZyboUARTState *state, u16 UartIntrId, Xil_ExceptionHandler handler);
+void uart_interrupt_handler(struct ZyboUARTState *state);
 
 int zybo_uart_reset(struct UARTDriver *self) {
-  // Instantiate the Driver state
-  if (self->state == NULL) {
-    self->state = malloc(sizeof(XUartPs));
-    if (self->state == NULL) {
-      return -1;
-    }
-    queue_init(&queue, buff, MAX_UART_BUFFER_SIZE);
-  }
+  // Ensure all required memory is allocated
+  struct ZyboUARTState *state = self->state;
+  if (state == NULL) return -1;
+  if (state->inst == NULL) return -1;
+  if (state->queue == NULL) return -1;
 
-  XUartPs *inst = self->state;;
+  XUartPs *inst = state->inst;
 
   // Configure XUartPs instance
   XUartPs_Config* config = XUartPs_LookupConfig(XPAR_PS7_UART_0_DEVICE_ID);
@@ -41,7 +32,7 @@ int zybo_uart_reset(struct UARTDriver *self) {
   *uart_ctrl_reg |= 0x00000003; // clear TX & RX
 
   // Setup Interrupts
-  if (SetupInterruptSystem(inst, XPAR_PS7_UART_0_INTR, (Xil_ExceptionHandler) uart_interrupt_handler) != XST_SUCCESS) {
+  if (SetupInterruptSystem(state, XPAR_PS7_UART_0_INTR, (Xil_ExceptionHandler) uart_interrupt_handler) != XST_SUCCESS) {
     return -1;
   }
 
@@ -68,14 +59,17 @@ int zybo_uart_reset(struct UARTDriver *self) {
   return 0;
 }
 
-int zybo_uart_write(struct UARTDriver *self, unsigned char c) {\
-  XUartPs *inst = self->state;
+int zybo_uart_write(struct UARTDriver *self, unsigned char c) {
+  struct ZyboUARTState *state = self->state;
+  XUartPs *inst = state->inst;
   XUartPs_SendByte(inst->Config.BaseAddress, c);
   return 0;
 }
 
 int zybo_uart_read(struct UARTDriver *self, unsigned char *c) {
-  if (queue_remove(&queue, c)) return -1;
+  struct ZyboUARTState *state = self->state;
+  struct Queue *queue = state->queue;
+  if (queue_remove(queue, c)) return -1;
   else return 0;
 }
 
@@ -188,8 +182,10 @@ int XUartPs_SetBaudRate_ours(XUartPs *InstancePtr, u32 BaudRate)
 
 }
 
-void uart_interrupt_handler(XUartPs *InstancePtr) {
+void uart_interrupt_handler(struct ZyboUARTState *state) {
   u32 IsrStatus;
+  XUartPs *InstancePtr = state->inst;
+  struct Queue *queue = state->queue;
 
   /*
    * Read the interrupt ID register to determine which
@@ -211,7 +207,7 @@ void uart_interrupt_handler(XUartPs *InstancePtr) {
 
   while (0 == (CsrRegister & XUARTPS_SR_RXEMPTY)) {
     u8 byte = XUartPs_ReadReg(InstancePtr->Config.BaseAddress, XUARTPS_FIFO_OFFSET);
-    queue_add(&queue, byte);
+    queue_add(queue, byte);
     CsrRegister = XUartPs_ReadReg(InstancePtr->Config.BaseAddress, XUARTPS_SR_OFFSET);
   }
 
@@ -238,10 +234,9 @@ void uart_interrupt_handler(XUartPs *InstancePtr) {
 * @note		None.
 *
 ****************************************************************************/
-int SetupInterruptSystem(XUartPs *UartInstancePtr, u16 UartIntrId, Xil_ExceptionHandler handler)
+int SetupInterruptSystem(struct ZyboUARTState *state, u16 UartIntrId, Xil_ExceptionHandler handler)
 {
 	int Status;
-
 	XScuGic_Config *IntcConfig; /* Config for interrupt controller */
 
 	/* Initialize the interrupt controller driver */
@@ -250,7 +245,8 @@ int SetupInterruptSystem(XUartPs *UartInstancePtr, u16 UartIntrId, Xil_Exception
 		return XST_FAILURE;
 	}
 
-	Status = XScuGic_CfgInitialize(&xscugic, IntcConfig,
+    memset(&state->xscugic, 0, sizeof(XScuGic));
+	Status = XScuGic_CfgInitialize(&state->xscugic, IntcConfig,
 					IntcConfig->CpuBaseAddress);
 	if (Status != XST_SUCCESS) {
 		return XST_FAILURE;
@@ -262,22 +258,22 @@ int SetupInterruptSystem(XUartPs *UartInstancePtr, u16 UartIntrId, Xil_Exception
 	 */
 	Xil_ExceptionRegisterHandler(XIL_EXCEPTION_ID_INT,
 				(Xil_ExceptionHandler) XScuGic_InterruptHandler,
-				&xscugic);
+				&state->xscugic);
 
 	/*
 	 * 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(&xscugic, UartIntrId,
+	Status = XScuGic_Connect(&state->xscugic, UartIntrId,
 				  handler,
-				  (void *) UartInstancePtr);
+				  (void *) state);
 	if (Status != XST_SUCCESS) {
 		return XST_FAILURE;
 	}
 
 	/* Enable the interrupt for the device */
-	XScuGic_Enable(&xscugic, UartIntrId);
+	XScuGic_Enable(&state->xscugic, UartIntrId);
 
 	/* Enable interrupts */
 	 Xil_ExceptionEnable();
diff --git a/quad/xsdk_workspace/real_quad/src/main.c b/quad/xsdk_workspace/real_quad/src/main.c
index 6ae2ffb1c23af2f3da60e56db12eb2afa79b6cfa..d715d8e6538906b99b38f21f50260d801540c4cf 100644
--- a/quad/xsdk_workspace/real_quad/src/main.c
+++ b/quad/xsdk_workspace/real_quad/src/main.c
@@ -6,10 +6,28 @@
 
 //#define RUN_TESTS
 
+/**
+ * Create the hardware drivers, and place them on the hardware struct.
+ *
+ * Things to keep in mind:
+ * - The CommDriver requires a UART. Hence, make the a UARTDriver first
+ *   and then give that instance to the CommDriver.
+ * - Same for GPSDriver, except it needs a different hardware device.
+ * - The I2C devices (IMU, LIDAR, Optical Flow), need a I2C bus. Hence
+ *   make a I2CDriver first, then give the appropariate instance the
+ *   device drivers.
+ *   - Currently, we are putting the Lidar on its own bus. Apparently,
+ *     when looking at an OScope with the Lidar on a bus with other
+ *     devices, it does not look right. The SCL line would never
+ *     go all the way back down to 0 volts
+ */
 int setup_hardware(hardware_t *hardware) {
   hardware->rc_receiver = create_zybo_rc_receiver();
   hardware->motors = create_zybo_motors();
-  hardware->uart = create_zybo_uart();
+  hardware->uart_0 = create_zybo_uart(0);
+  hardware->uart_1 = create_zybo_uart(1);
+  hardware->comm = create_zybo_comm(&hardware->uart_0);
+  hardware->gps = create_zybo_gps(&hardware->uart_1);
   hardware->global_timer = create_zybo_global_timer();
   hardware->axi_timer = create_zybo_axi_timer();
   hardware->mio7_led = create_zybo_mio7_led();
@@ -35,10 +53,10 @@ int main()
   //test_zybo_i2c_lidar();
   //test_zybo_i2c_all();
   //test_zybo_rc_receiver();
-  test_zybo_motors();
+  //test_zybo_motors();
   //test_zybo_uart();
   //test_zybo_axi_timer();
-  //test_zybo_uart();
+  test_zybo_uart_comm();
   return 0;
 #endif
 
diff --git a/quad/xsdk_workspace/system_bsp/.cproject b/quad/xsdk_workspace/system_bsp/.cproject
index 58ab3351cc7a84b2307d26a9be45e62c628cc66f..d95314090b5587f6375b20955412f1ac382bb324 100644
--- a/quad/xsdk_workspace/system_bsp/.cproject
+++ b/quad/xsdk_workspace/system_bsp/.cproject
@@ -3,8 +3,8 @@
 
 <cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
 	<storageModule moduleId="org.eclipse.cdt.core.settings">
-		<cconfiguration id="org.eclipse.cdt.core.default.config.946402869">
-			<storageModule buildSystemId="org.eclipse.cdt.core.defaultConfigDataProvider" id="org.eclipse.cdt.core.default.config.946402869" moduleId="org.eclipse.cdt.core.settings" name="Configuration">
+		<cconfiguration id="org.eclipse.cdt.core.default.config.154473096">
+			<storageModule buildSystemId="org.eclipse.cdt.core.defaultConfigDataProvider" id="org.eclipse.cdt.core.default.config.154473096" moduleId="org.eclipse.cdt.core.settings" name="Configuration">
 				<externalSettings/>
 				<extensions/>
 			</storageModule>
diff --git a/quad/xsdk_workspace/system_bsp/system.mss b/quad/xsdk_workspace/system_bsp/system.mss
index 2c47ed63fb85c6b87fabec00cacc13c5ae4fa666..c68ab7b92b15f34acd7f5630013691eb388335e3 100644
--- a/quad/xsdk_workspace/system_bsp/system.mss
+++ b/quad/xsdk_workspace/system_bsp/system.mss
@@ -15,6 +15,7 @@ BEGIN PROCESSOR
  PARAMETER DRIVER_NAME = cpu_cortexa9
  PARAMETER DRIVER_VER = 1.01.a
  PARAMETER HW_INSTANCE = ps7_cortexa9_0
+ PARAMETER EXTRA_COMPILER_FLAGS = -O0 -g
 END
 
 
diff --git a/quad/xsdk_workspace/zybo_fsbl/.cproject b/quad/xsdk_workspace/zybo_fsbl/.cproject
index 7ddad3fcbf9d707d11bd9f0bb6f2d6a36f1c41c2..1434e38d4d1165e75cdc6c29b1a70a808484531f 100644
--- a/quad/xsdk_workspace/zybo_fsbl/.cproject
+++ b/quad/xsdk_workspace/zybo_fsbl/.cproject
@@ -3,8 +3,8 @@
 
 <cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
 	<storageModule moduleId="org.eclipse.cdt.core.settings">
-		<cconfiguration id="xilinx.gnu.arm.exe.debug.1528513972">
-			<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="xilinx.gnu.arm.exe.debug.1528513972" moduleId="org.eclipse.cdt.core.settings" name="Debug">
+		<cconfiguration id="xilinx.gnu.arm.exe.debug.631125449">
+			<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="xilinx.gnu.arm.exe.debug.631125449" moduleId="org.eclipse.cdt.core.settings" name="Debug">
 				<externalSettings/>
 				<extensions>
 					<extension id="com.xilinx.sdk.managedbuilder.XELF.arm" point="org.eclipse.cdt.core.BinaryParser"/>
@@ -16,68 +16,68 @@
 				</extensions>
 			</storageModule>
 			<storageModule moduleId="cdtBuildSystem" version="4.0.0">
-				<configuration artifactExtension="elf" artifactName="${ProjName}" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.debug,org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe" cleanCommand="rm -rf" description="" id="xilinx.gnu.arm.exe.debug.1528513972" name="Debug" parent="xilinx.gnu.arm.exe.debug">
-					<folderInfo id="xilinx.gnu.arm.exe.debug.1528513972." name="/" resourcePath="">
-						<toolChain id="xilinx.gnu.arm.exe.debug.toolchain.620865263" name="Xilinx ARM GNU Toolchain" superClass="xilinx.gnu.arm.exe.debug.toolchain">
-							<targetPlatform binaryParser="com.xilinx.sdk.managedbuilder.XELF.arm" id="xilinx.arm.target.gnu.base.debug.1299613950" isAbstract="false" name="Debug Platform" superClass="xilinx.arm.target.gnu.base.debug"/>
-							<builder buildPath="${workspace_loc:/zybo_fsbl}/Debug" enableAutoBuild="true" id="xilinx.gnu.arm.toolchain.builder.debug.1331514252" managedBuildOn="true" name="GNU make.Debug" superClass="xilinx.gnu.arm.toolchain.builder.debug"/>
-							<tool id="xilinx.gnu.arm.c.toolchain.assembler.debug.911778202" name="ARM gcc assembler" superClass="xilinx.gnu.arm.c.toolchain.assembler.debug">
-								<inputType id="xilinx.gnu.assembler.input.2001882527" superClass="xilinx.gnu.assembler.input"/>
+				<configuration artifactExtension="elf" artifactName="${ProjName}" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.debug,org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe" cleanCommand="rm -rf" description="" id="xilinx.gnu.arm.exe.debug.631125449" name="Debug" parent="xilinx.gnu.arm.exe.debug">
+					<folderInfo id="xilinx.gnu.arm.exe.debug.631125449." name="/" resourcePath="">
+						<toolChain id="xilinx.gnu.arm.exe.debug.toolchain.598522392" name="Xilinx ARM GNU Toolchain" superClass="xilinx.gnu.arm.exe.debug.toolchain">
+							<targetPlatform binaryParser="com.xilinx.sdk.managedbuilder.XELF.arm" id="xilinx.arm.target.gnu.base.debug.1372637876" isAbstract="false" name="Debug Platform" superClass="xilinx.arm.target.gnu.base.debug"/>
+							<builder buildPath="${workspace_loc:/zybo_fsbl}/Debug" enableAutoBuild="true" id="xilinx.gnu.arm.toolchain.builder.debug.2066333847" managedBuildOn="true" name="GNU make.Debug" superClass="xilinx.gnu.arm.toolchain.builder.debug"/>
+							<tool id="xilinx.gnu.arm.c.toolchain.assembler.debug.792124132" name="ARM gcc assembler" superClass="xilinx.gnu.arm.c.toolchain.assembler.debug">
+								<inputType id="xilinx.gnu.assembler.input.1066651972" superClass="xilinx.gnu.assembler.input"/>
 							</tool>
-							<tool id="xilinx.gnu.arm.c.toolchain.compiler.debug.1840889584" name="ARM gcc compiler" superClass="xilinx.gnu.arm.c.toolchain.compiler.debug">
-								<option defaultValue="gnu.c.optimization.level.none" id="xilinx.gnu.compiler.option.optimization.level.2110622919" superClass="xilinx.gnu.compiler.option.optimization.level" valueType="enumerated"/>
-								<option id="xilinx.gnu.compiler.option.debugging.level.1324950494" superClass="xilinx.gnu.compiler.option.debugging.level" value="gnu.c.debugging.level.max" valueType="enumerated"/>
-								<option id="xilinx.gnu.compiler.inferred.swplatform.includes.1010106927" superClass="xilinx.gnu.compiler.inferred.swplatform.includes" valueType="includePath">
+							<tool id="xilinx.gnu.arm.c.toolchain.compiler.debug.371418732" name="ARM gcc compiler" superClass="xilinx.gnu.arm.c.toolchain.compiler.debug">
+								<option defaultValue="gnu.c.optimization.level.none" id="xilinx.gnu.compiler.option.optimization.level.1709273506" superClass="xilinx.gnu.compiler.option.optimization.level" valueType="enumerated"/>
+								<option id="xilinx.gnu.compiler.option.debugging.level.1095090898" superClass="xilinx.gnu.compiler.option.debugging.level" value="gnu.c.debugging.level.max" valueType="enumerated"/>
+								<option id="xilinx.gnu.compiler.inferred.swplatform.includes.2076701770" superClass="xilinx.gnu.compiler.inferred.swplatform.includes" valueType="includePath">
 									<listOptionValue builtIn="false" value="../../zybo_fsbl_bsp/ps7_cortexa9_0/include"/>
 								</option>
-								<inputType id="xilinx.gnu.arm.c.compiler.input.164824730" name="C source files" superClass="xilinx.gnu.arm.c.compiler.input"/>
+								<inputType id="xilinx.gnu.arm.c.compiler.input.670667181" name="C source files" superClass="xilinx.gnu.arm.c.compiler.input"/>
 							</tool>
-							<tool id="xilinx.gnu.arm.cxx.toolchain.compiler.debug.247997005" name="ARM g++ compiler" superClass="xilinx.gnu.arm.cxx.toolchain.compiler.debug">
-								<option defaultValue="gnu.c.optimization.level.none" id="xilinx.gnu.compiler.option.optimization.level.1779591449" superClass="xilinx.gnu.compiler.option.optimization.level" valueType="enumerated"/>
-								<option id="xilinx.gnu.compiler.option.debugging.level.1320442366" superClass="xilinx.gnu.compiler.option.debugging.level" value="gnu.c.debugging.level.max" valueType="enumerated"/>
-								<option id="xilinx.gnu.compiler.inferred.swplatform.includes.1122443388" superClass="xilinx.gnu.compiler.inferred.swplatform.includes" valueType="includePath">
+							<tool id="xilinx.gnu.arm.cxx.toolchain.compiler.debug.153994272" name="ARM g++ compiler" superClass="xilinx.gnu.arm.cxx.toolchain.compiler.debug">
+								<option defaultValue="gnu.c.optimization.level.none" id="xilinx.gnu.compiler.option.optimization.level.273904714" superClass="xilinx.gnu.compiler.option.optimization.level" valueType="enumerated"/>
+								<option id="xilinx.gnu.compiler.option.debugging.level.100308615" superClass="xilinx.gnu.compiler.option.debugging.level" value="gnu.c.debugging.level.max" valueType="enumerated"/>
+								<option id="xilinx.gnu.compiler.inferred.swplatform.includes.1777219656" superClass="xilinx.gnu.compiler.inferred.swplatform.includes" valueType="includePath">
 									<listOptionValue builtIn="false" value="../../zybo_fsbl_bsp/ps7_cortexa9_0/include"/>
 								</option>
 							</tool>
-							<tool id="xilinx.gnu.arm.toolchain.archiver.1400731677" name="ARM archiver" superClass="xilinx.gnu.arm.toolchain.archiver"/>
-							<tool id="xilinx.gnu.arm.c.toolchain.linker.debug.345997425" name="ARM gcc linker" superClass="xilinx.gnu.arm.c.toolchain.linker.debug">
-								<option id="xilinx.gnu.linker.inferred.swplatform.lpath.1716168836" superClass="xilinx.gnu.linker.inferred.swplatform.lpath" valueType="libPaths">
+							<tool id="xilinx.gnu.arm.toolchain.archiver.15429244" name="ARM archiver" superClass="xilinx.gnu.arm.toolchain.archiver"/>
+							<tool id="xilinx.gnu.arm.c.toolchain.linker.debug.1131508766" name="ARM gcc linker" superClass="xilinx.gnu.arm.c.toolchain.linker.debug">
+								<option id="xilinx.gnu.linker.inferred.swplatform.lpath.1222044113" superClass="xilinx.gnu.linker.inferred.swplatform.lpath" valueType="libPaths">
 									<listOptionValue builtIn="false" value="../../zybo_fsbl_bsp/ps7_cortexa9_0/lib"/>
 								</option>
-								<option id="xilinx.gnu.linker.inferred.swplatform.flags.1873644137" superClass="xilinx.gnu.linker.inferred.swplatform.flags" valueType="libs">
+								<option id="xilinx.gnu.linker.inferred.swplatform.flags.13428014" superClass="xilinx.gnu.linker.inferred.swplatform.flags" valueType="libs">
 									<listOptionValue builtIn="false" value="-Wl,--start-group,-lxil,-lgcc,-lc,--end-group"/>
 								</option>
-								<option id="xilinx.gnu.c.linker.option.lscript.1950983133" superClass="xilinx.gnu.c.linker.option.lscript" value="../src/lscript.ld" valueType="string"/>
-								<option id="xilinx.gnu.c.link.option.libs.598950432" superClass="xilinx.gnu.c.link.option.libs" valueType="libs">
+								<option id="xilinx.gnu.c.linker.option.lscript.1931360495" superClass="xilinx.gnu.c.linker.option.lscript" value="../src/lscript.ld" valueType="string"/>
+								<option id="xilinx.gnu.c.link.option.libs.1547145650" superClass="xilinx.gnu.c.link.option.libs" valueType="libs">
 									<listOptionValue builtIn="false" value="rsa"/>
 								</option>
-								<option id="xilinx.gnu.c.link.option.paths.58794466" superClass="xilinx.gnu.c.link.option.paths" valueType="libPaths">
+								<option id="xilinx.gnu.c.link.option.paths.999363621" superClass="xilinx.gnu.c.link.option.paths" valueType="libPaths">
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/src}&quot;"/>
 								</option>
-								<inputType id="xilinx.gnu.linker.input.103080881" superClass="xilinx.gnu.linker.input">
+								<inputType id="xilinx.gnu.linker.input.1436307817" superClass="xilinx.gnu.linker.input">
 									<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
 									<additionalInput kind="additionalinput" paths="$(LIBS)"/>
 								</inputType>
-								<inputType id="xilinx.gnu.linker.input.lscript.428167088" name="Linker Script" superClass="xilinx.gnu.linker.input.lscript"/>
+								<inputType id="xilinx.gnu.linker.input.lscript.1950636018" name="Linker Script" superClass="xilinx.gnu.linker.input.lscript"/>
 							</tool>
-							<tool id="xilinx.gnu.arm.cxx.toolchain.linker.debug.566442706" name="ARM g++ linker" superClass="xilinx.gnu.arm.cxx.toolchain.linker.debug">
-								<option id="xilinx.gnu.linker.inferred.swplatform.lpath.1863640969" superClass="xilinx.gnu.linker.inferred.swplatform.lpath" valueType="libPaths">
+							<tool id="xilinx.gnu.arm.cxx.toolchain.linker.debug.703659759" name="ARM g++ linker" superClass="xilinx.gnu.arm.cxx.toolchain.linker.debug">
+								<option id="xilinx.gnu.linker.inferred.swplatform.lpath.800774044" superClass="xilinx.gnu.linker.inferred.swplatform.lpath" valueType="libPaths">
 									<listOptionValue builtIn="false" value="../../zybo_fsbl_bsp/ps7_cortexa9_0/lib"/>
 								</option>
-								<option id="xilinx.gnu.linker.inferred.swplatform.flags.2096513062" superClass="xilinx.gnu.linker.inferred.swplatform.flags" valueType="libs">
+								<option id="xilinx.gnu.linker.inferred.swplatform.flags.437956417" superClass="xilinx.gnu.linker.inferred.swplatform.flags" valueType="libs">
 									<listOptionValue builtIn="false" value="-Wl,--start-group,-lxil,-lgcc,-lc,--end-group"/>
 								</option>
-								<option id="xilinx.gnu.c.linker.option.lscript.1103422420" superClass="xilinx.gnu.c.linker.option.lscript" value="../src/lscript.ld" valueType="string"/>
+								<option id="xilinx.gnu.c.linker.option.lscript.118016047" superClass="xilinx.gnu.c.linker.option.lscript" value="../src/lscript.ld" valueType="string"/>
 							</tool>
-							<tool id="xilinx.gnu.arm.size.debug.2048630709" name="ARM Print Size" superClass="xilinx.gnu.arm.size.debug"/>
+							<tool id="xilinx.gnu.arm.size.debug.569464936" name="ARM Print Size" superClass="xilinx.gnu.arm.size.debug"/>
 						</toolChain>
 					</folderInfo>
 				</configuration>
 			</storageModule>
 			<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
 		</cconfiguration>
-		<cconfiguration id="xilinx.gnu.arm.exe.release.819164381">
-			<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="xilinx.gnu.arm.exe.release.819164381" moduleId="org.eclipse.cdt.core.settings" name="Release">
+		<cconfiguration id="xilinx.gnu.arm.exe.release.594409871">
+			<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="xilinx.gnu.arm.exe.release.594409871" moduleId="org.eclipse.cdt.core.settings" name="Release">
 				<externalSettings/>
 				<extensions>
 					<extension id="com.xilinx.sdk.managedbuilder.XELF.arm" point="org.eclipse.cdt.core.BinaryParser"/>
@@ -89,60 +89,60 @@
 				</extensions>
 			</storageModule>
 			<storageModule moduleId="cdtBuildSystem" version="4.0.0">
-				<configuration artifactExtension="elf" artifactName="${ProjName}" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.release,org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe" cleanCommand="rm -rf" description="" id="xilinx.gnu.arm.exe.release.819164381" name="Release" parent="xilinx.gnu.arm.exe.release">
-					<folderInfo id="xilinx.gnu.arm.exe.release.819164381." name="/" resourcePath="">
-						<toolChain id="xilinx.gnu.arm.exe.release.toolchain.1240523721" name="Xilinx ARM GNU Toolchain" superClass="xilinx.gnu.arm.exe.release.toolchain">
-							<targetPlatform binaryParser="com.xilinx.sdk.managedbuilder.XELF.arm" id="xilinx.arm.target.gnu.base.release.1592659856" isAbstract="false" name="Release Platform" superClass="xilinx.arm.target.gnu.base.release"/>
-							<builder buildPath="${workspace_loc:/zybo_fsbl}/Release" enableAutoBuild="true" id="xilinx.gnu.arm.toolchain.builder.release.220781107" managedBuildOn="true" name="GNU make.Release" superClass="xilinx.gnu.arm.toolchain.builder.release"/>
-							<tool id="xilinx.gnu.arm.c.toolchain.assembler.release.1719774153" name="ARM gcc assembler" superClass="xilinx.gnu.arm.c.toolchain.assembler.release">
-								<inputType id="xilinx.gnu.assembler.input.431288829" superClass="xilinx.gnu.assembler.input"/>
+				<configuration artifactExtension="elf" artifactName="${ProjName}" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.release,org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe" cleanCommand="rm -rf" description="" id="xilinx.gnu.arm.exe.release.594409871" name="Release" parent="xilinx.gnu.arm.exe.release">
+					<folderInfo id="xilinx.gnu.arm.exe.release.594409871." name="/" resourcePath="">
+						<toolChain id="xilinx.gnu.arm.exe.release.toolchain.507750734" name="Xilinx ARM GNU Toolchain" superClass="xilinx.gnu.arm.exe.release.toolchain">
+							<targetPlatform binaryParser="com.xilinx.sdk.managedbuilder.XELF.arm" id="xilinx.arm.target.gnu.base.release.1628399965" isAbstract="false" name="Release Platform" superClass="xilinx.arm.target.gnu.base.release"/>
+							<builder buildPath="${workspace_loc:/zybo_fsbl}/Release" enableAutoBuild="true" id="xilinx.gnu.arm.toolchain.builder.release.1422012668" managedBuildOn="true" name="GNU make.Release" superClass="xilinx.gnu.arm.toolchain.builder.release"/>
+							<tool id="xilinx.gnu.arm.c.toolchain.assembler.release.865141912" name="ARM gcc assembler" superClass="xilinx.gnu.arm.c.toolchain.assembler.release">
+								<inputType id="xilinx.gnu.assembler.input.1671779125" superClass="xilinx.gnu.assembler.input"/>
 							</tool>
-							<tool id="xilinx.gnu.arm.c.toolchain.compiler.release.2059219877" name="ARM gcc compiler" superClass="xilinx.gnu.arm.c.toolchain.compiler.release">
-								<option defaultValue="gnu.c.optimization.level.more" id="xilinx.gnu.compiler.option.optimization.level.1308162708" superClass="xilinx.gnu.compiler.option.optimization.level" valueType="enumerated"/>
-								<option id="xilinx.gnu.compiler.option.debugging.level.1671222344" superClass="xilinx.gnu.compiler.option.debugging.level" value="gnu.c.debugging.level.none" valueType="enumerated"/>
-								<option id="xilinx.gnu.compiler.inferred.swplatform.includes.1131284540" superClass="xilinx.gnu.compiler.inferred.swplatform.includes" valueType="includePath">
+							<tool id="xilinx.gnu.arm.c.toolchain.compiler.release.2101352431" name="ARM gcc compiler" superClass="xilinx.gnu.arm.c.toolchain.compiler.release">
+								<option defaultValue="gnu.c.optimization.level.more" id="xilinx.gnu.compiler.option.optimization.level.1644059076" superClass="xilinx.gnu.compiler.option.optimization.level" valueType="enumerated"/>
+								<option id="xilinx.gnu.compiler.option.debugging.level.1522632993" superClass="xilinx.gnu.compiler.option.debugging.level" value="gnu.c.debugging.level.none" valueType="enumerated"/>
+								<option id="xilinx.gnu.compiler.inferred.swplatform.includes.1242558173" superClass="xilinx.gnu.compiler.inferred.swplatform.includes" valueType="includePath">
 									<listOptionValue builtIn="false" value="../../zybo_fsbl_bsp/ps7_cortexa9_0/include"/>
 								</option>
-								<inputType id="xilinx.gnu.arm.c.compiler.input.1631463841" name="C source files" superClass="xilinx.gnu.arm.c.compiler.input"/>
+								<inputType id="xilinx.gnu.arm.c.compiler.input.729397047" name="C source files" superClass="xilinx.gnu.arm.c.compiler.input"/>
 							</tool>
-							<tool id="xilinx.gnu.arm.cxx.toolchain.compiler.release.891744552" name="ARM g++ compiler" superClass="xilinx.gnu.arm.cxx.toolchain.compiler.release">
-								<option defaultValue="gnu.c.optimization.level.more" id="xilinx.gnu.compiler.option.optimization.level.1151102494" superClass="xilinx.gnu.compiler.option.optimization.level" valueType="enumerated"/>
-								<option id="xilinx.gnu.compiler.option.debugging.level.1077797144" superClass="xilinx.gnu.compiler.option.debugging.level" value="gnu.c.debugging.level.none" valueType="enumerated"/>
-								<option id="xilinx.gnu.compiler.inferred.swplatform.includes.1913917319" superClass="xilinx.gnu.compiler.inferred.swplatform.includes" valueType="includePath">
+							<tool id="xilinx.gnu.arm.cxx.toolchain.compiler.release.825857438" name="ARM g++ compiler" superClass="xilinx.gnu.arm.cxx.toolchain.compiler.release">
+								<option defaultValue="gnu.c.optimization.level.more" id="xilinx.gnu.compiler.option.optimization.level.1953632681" superClass="xilinx.gnu.compiler.option.optimization.level" valueType="enumerated"/>
+								<option id="xilinx.gnu.compiler.option.debugging.level.2062160021" superClass="xilinx.gnu.compiler.option.debugging.level" value="gnu.c.debugging.level.none" valueType="enumerated"/>
+								<option id="xilinx.gnu.compiler.inferred.swplatform.includes.978611856" superClass="xilinx.gnu.compiler.inferred.swplatform.includes" valueType="includePath">
 									<listOptionValue builtIn="false" value="../../zybo_fsbl_bsp/ps7_cortexa9_0/include"/>
 								</option>
 							</tool>
-							<tool id="xilinx.gnu.arm.toolchain.archiver.1118360987" name="ARM archiver" superClass="xilinx.gnu.arm.toolchain.archiver"/>
-							<tool id="xilinx.gnu.arm.c.toolchain.linker.release.1132751753" name="ARM gcc linker" superClass="xilinx.gnu.arm.c.toolchain.linker.release">
-								<option id="xilinx.gnu.linker.inferred.swplatform.lpath.1917836743" superClass="xilinx.gnu.linker.inferred.swplatform.lpath" valueType="libPaths">
+							<tool id="xilinx.gnu.arm.toolchain.archiver.297211059" name="ARM archiver" superClass="xilinx.gnu.arm.toolchain.archiver"/>
+							<tool id="xilinx.gnu.arm.c.toolchain.linker.release.1225963780" name="ARM gcc linker" superClass="xilinx.gnu.arm.c.toolchain.linker.release">
+								<option id="xilinx.gnu.linker.inferred.swplatform.lpath.1662060075" superClass="xilinx.gnu.linker.inferred.swplatform.lpath" valueType="libPaths">
 									<listOptionValue builtIn="false" value="../../zybo_fsbl_bsp/ps7_cortexa9_0/lib"/>
 								</option>
-								<option id="xilinx.gnu.linker.inferred.swplatform.flags.383672501" superClass="xilinx.gnu.linker.inferred.swplatform.flags" valueType="libs">
+								<option id="xilinx.gnu.linker.inferred.swplatform.flags.1712694149" superClass="xilinx.gnu.linker.inferred.swplatform.flags" valueType="libs">
 									<listOptionValue builtIn="false" value="-Wl,--start-group,-lxil,-lgcc,-lc,--end-group"/>
 								</option>
-								<option id="xilinx.gnu.c.linker.option.lscript.1608063800" superClass="xilinx.gnu.c.linker.option.lscript" value="../src/lscript.ld" valueType="string"/>
-								<option id="xilinx.gnu.c.link.option.libs.1281241736" superClass="xilinx.gnu.c.link.option.libs" valueType="libs">
+								<option id="xilinx.gnu.c.linker.option.lscript.907765905" superClass="xilinx.gnu.c.linker.option.lscript" value="../src/lscript.ld" valueType="string"/>
+								<option id="xilinx.gnu.c.link.option.libs.2026302928" superClass="xilinx.gnu.c.link.option.libs" valueType="libs">
 									<listOptionValue builtIn="false" value="rsa"/>
 								</option>
-								<option id="xilinx.gnu.c.link.option.paths.40091586" superClass="xilinx.gnu.c.link.option.paths" valueType="libPaths">
+								<option id="xilinx.gnu.c.link.option.paths.1361224741" superClass="xilinx.gnu.c.link.option.paths" valueType="libPaths">
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/src}&quot;"/>
 								</option>
-								<inputType id="xilinx.gnu.linker.input.926037696" superClass="xilinx.gnu.linker.input">
+								<inputType id="xilinx.gnu.linker.input.749170694" superClass="xilinx.gnu.linker.input">
 									<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
 									<additionalInput kind="additionalinput" paths="$(LIBS)"/>
 								</inputType>
-								<inputType id="xilinx.gnu.linker.input.lscript.1292679303" name="Linker Script" superClass="xilinx.gnu.linker.input.lscript"/>
+								<inputType id="xilinx.gnu.linker.input.lscript.503889234" name="Linker Script" superClass="xilinx.gnu.linker.input.lscript"/>
 							</tool>
-							<tool id="xilinx.gnu.arm.cxx.toolchain.linker.release.823586348" name="ARM g++ linker" superClass="xilinx.gnu.arm.cxx.toolchain.linker.release">
-								<option id="xilinx.gnu.linker.inferred.swplatform.lpath.1966466748" superClass="xilinx.gnu.linker.inferred.swplatform.lpath" valueType="libPaths">
+							<tool id="xilinx.gnu.arm.cxx.toolchain.linker.release.659189711" name="ARM g++ linker" superClass="xilinx.gnu.arm.cxx.toolchain.linker.release">
+								<option id="xilinx.gnu.linker.inferred.swplatform.lpath.1372604190" superClass="xilinx.gnu.linker.inferred.swplatform.lpath" valueType="libPaths">
 									<listOptionValue builtIn="false" value="../../zybo_fsbl_bsp/ps7_cortexa9_0/lib"/>
 								</option>
-								<option id="xilinx.gnu.linker.inferred.swplatform.flags.1228570640" superClass="xilinx.gnu.linker.inferred.swplatform.flags" valueType="libs">
+								<option id="xilinx.gnu.linker.inferred.swplatform.flags.1310678011" superClass="xilinx.gnu.linker.inferred.swplatform.flags" valueType="libs">
 									<listOptionValue builtIn="false" value="-Wl,--start-group,-lxil,-lgcc,-lc,--end-group"/>
 								</option>
-								<option id="xilinx.gnu.c.linker.option.lscript.1403296691" superClass="xilinx.gnu.c.linker.option.lscript" value="../src/lscript.ld" valueType="string"/>
+								<option id="xilinx.gnu.c.linker.option.lscript.1404470723" superClass="xilinx.gnu.c.linker.option.lscript" value="../src/lscript.ld" valueType="string"/>
 							</tool>
-							<tool id="xilinx.gnu.arm.size.release.1130359049" name="ARM Print Size" superClass="xilinx.gnu.arm.size.release"/>
+							<tool id="xilinx.gnu.arm.size.release.108428989" name="ARM Print Size" superClass="xilinx.gnu.arm.size.release"/>
 						</toolChain>
 					</folderInfo>
 				</configuration>
@@ -152,20 +152,20 @@
 	</storageModule>
 	<storageModule moduleId="org.eclipse.cdt.core.LanguageSettingsProviders"/>
 	<storageModule moduleId="cdtBuildSystem" version="4.0.0">
-		<project id="zybo_fsbl.xilinx.gnu.arm.exe.36875234" name="Xilinx ARM Executable" projectType="xilinx.gnu.arm.exe"/>
+		<project id="zybo_fsbl.xilinx.gnu.arm.exe.117806380" name="Xilinx ARM Executable" projectType="xilinx.gnu.arm.exe"/>
 	</storageModule>
 	<storageModule moduleId="scannerConfiguration">
 		<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
-		<scannerConfigBuildInfo instanceId="xilinx.gnu.arm.exe.debug.1528513972;xilinx.gnu.arm.exe.debug.1528513972.;xilinx.gnu.arm.c.toolchain.compiler.debug.1840889584;xilinx.gnu.arm.c.compiler.input.164824730">
+		<scannerConfigBuildInfo instanceId="xilinx.gnu.arm.exe.release.594409871;xilinx.gnu.arm.exe.release.594409871.;xilinx.gnu.arm.c.toolchain.compiler.release.2101352431;xilinx.gnu.arm.c.compiler.input.729397047">
 			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="com.xilinx.managedbuilder.ui.ARMGCCManagedMakePerProjectProfileC"/>
 		</scannerConfigBuildInfo>
-		<scannerConfigBuildInfo instanceId="xilinx.gnu.arm.exe.release.819164381;xilinx.gnu.arm.exe.release.819164381.;xilinx.gnu.arm.c.toolchain.compiler.release.2059219877;xilinx.gnu.arm.c.compiler.input.1631463841">
+		<scannerConfigBuildInfo instanceId="xilinx.gnu.arm.exe.debug.631125449;xilinx.gnu.arm.exe.debug.631125449.">
 			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="com.xilinx.managedbuilder.ui.ARMGCCManagedMakePerProjectProfileC"/>
 		</scannerConfigBuildInfo>
-		<scannerConfigBuildInfo instanceId="xilinx.gnu.arm.exe.release.819164381;xilinx.gnu.arm.exe.release.819164381.">
+		<scannerConfigBuildInfo instanceId="xilinx.gnu.arm.exe.release.594409871;xilinx.gnu.arm.exe.release.594409871.">
 			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="com.xilinx.managedbuilder.ui.ARMGCCManagedMakePerProjectProfileC"/>
 		</scannerConfigBuildInfo>
-		<scannerConfigBuildInfo instanceId="xilinx.gnu.arm.exe.debug.1528513972;xilinx.gnu.arm.exe.debug.1528513972.">
+		<scannerConfigBuildInfo instanceId="xilinx.gnu.arm.exe.debug.631125449;xilinx.gnu.arm.exe.debug.631125449.;xilinx.gnu.arm.c.toolchain.compiler.debug.371418732;xilinx.gnu.arm.c.compiler.input.670667181">
 			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="com.xilinx.managedbuilder.ui.ARMGCCManagedMakePerProjectProfileC"/>
 		</scannerConfigBuildInfo>
 	</storageModule>
diff --git a/quad/xsdk_workspace/zybo_fsbl/Release/makefile b/quad/xsdk_workspace/zybo_fsbl/Release/makefile
deleted file mode 100644
index 97959e1ede4a1a3378f2241945e4d9dc184546ac..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/zybo_fsbl/Release/makefile
+++ /dev/null
@@ -1,58 +0,0 @@
-################################################################################
-# Automatically-generated file. Do not edit!
-################################################################################
-
--include ../makefile.init
-
-RM := rm -rf
-
-# All of the sources participating in the build are defined here
--include sources.mk
--include src/subdir.mk
--include subdir.mk
--include objects.mk
-
-ifneq ($(MAKECMDGOALS),clean)
-ifneq ($(strip $(C_DEPS)),)
--include $(C_DEPS)
-endif
-ifneq ($(strip $(S_UPPER_DEPS)),)
--include $(S_UPPER_DEPS)
-endif
-endif
-
--include ../makefile.defs
-
-# Add inputs and outputs from these tool invocations to the build variables 
-ELFSIZE += \
-zybo_fsbl.elf.size \
-
-
-# All Target
-all: zybo_fsbl.elf secondary-outputs
-
-# Tool invocations
-zybo_fsbl.elf: $(OBJS) ../src/lscript.ld $(USER_OBJS)
-	@echo 'Building target: $@'
-	@echo 'Invoking: ARM gcc linker'
-	arm-xilinx-eabi-gcc -L"/local/ucart/MicroCART_17-18/quad/xsdk_workspace/zybo_fsbl/src" -Wl,-T -Wl,../src/lscript.ld -L../../zybo_fsbl_bsp/ps7_cortexa9_0/lib -o "zybo_fsbl.elf" $(OBJS) $(USER_OBJS) $(LIBS)
-	@echo 'Finished building target: $@'
-	@echo ' '
-
-zybo_fsbl.elf.size: zybo_fsbl.elf
-	@echo 'Invoking: ARM Print Size'
-	arm-xilinx-eabi-size zybo_fsbl.elf  |tee "zybo_fsbl.elf.size"
-	@echo 'Finished building: $@'
-	@echo ' '
-
-# Other Targets
-clean:
-	-$(RM) $(OBJS)$(C_DEPS)$(EXECUTABLES)$(ELFSIZE)$(S_UPPER_DEPS) zybo_fsbl.elf
-	-@echo ' '
-
-secondary-outputs: $(ELFSIZE)
-
-.PHONY: all clean dependents
-.SECONDARY:
-
--include ../makefile.targets
diff --git a/quad/xsdk_workspace/zybo_fsbl/Release/objects.mk b/quad/xsdk_workspace/zybo_fsbl/Release/objects.mk
deleted file mode 100644
index 88ab2f86c60bc732322248eb99b64703856ff9af..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/zybo_fsbl/Release/objects.mk
+++ /dev/null
@@ -1,8 +0,0 @@
-################################################################################
-# Automatically-generated file. Do not edit!
-################################################################################
-
-USER_OBJS :=
-
-LIBS := -lrsa -Wl,--start-group,-lxil,-lgcc,-lc,--end-group
-
diff --git a/quad/xsdk_workspace/zybo_fsbl/Release/sources.mk b/quad/xsdk_workspace/zybo_fsbl/Release/sources.mk
deleted file mode 100644
index a7c54c38a1e61325feea8965fd7f659ed223d79b..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/zybo_fsbl/Release/sources.mk
+++ /dev/null
@@ -1,20 +0,0 @@
-################################################################################
-# Automatically-generated file. Do not edit!
-################################################################################
-
-O_SRCS := 
-C_SRCS := 
-LD_SRCS := 
-S_UPPER_SRCS := 
-S_SRCS := 
-OBJ_SRCS := 
-OBJS := 
-C_DEPS := 
-EXECUTABLES := 
-ELFSIZE := 
-S_UPPER_DEPS := 
-
-# Every subdirectory with source files must be described here
-SUBDIRS := \
-src \
-
diff --git a/quad/xsdk_workspace/zybo_fsbl/Release/src/subdir.mk b/quad/xsdk_workspace/zybo_fsbl/Release/src/subdir.mk
deleted file mode 100644
index c45bb40c493a6c0fde19fd2ef6eaa2600e0e6fbb..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/zybo_fsbl/Release/src/subdir.mk
+++ /dev/null
@@ -1,80 +0,0 @@
-################################################################################
-# Automatically-generated file. Do not edit!
-################################################################################
-
-# Add inputs and outputs from these tool invocations to the build variables 
-C_SRCS += \
-../src/ddr_init.c \
-../src/ff.c \
-../src/fsbl_hooks.c \
-../src/image_mover.c \
-../src/main.c \
-../src/md5.c \
-../src/mmc.c \
-../src/nand.c \
-../src/nor.c \
-../src/pcap.c \
-../src/ps7_init.c \
-../src/qspi.c \
-../src/rsa.c \
-../src/sd.c 
-
-LD_SRCS += \
-../src/lscript.ld 
-
-S_UPPER_SRCS += \
-../src/fsbl_handoff.S 
-
-OBJS += \
-./src/ddr_init.o \
-./src/ff.o \
-./src/fsbl_handoff.o \
-./src/fsbl_hooks.o \
-./src/image_mover.o \
-./src/main.o \
-./src/md5.o \
-./src/mmc.o \
-./src/nand.o \
-./src/nor.o \
-./src/pcap.o \
-./src/ps7_init.o \
-./src/qspi.o \
-./src/rsa.o \
-./src/sd.o 
-
-C_DEPS += \
-./src/ddr_init.d \
-./src/ff.d \
-./src/fsbl_hooks.d \
-./src/image_mover.d \
-./src/main.d \
-./src/md5.d \
-./src/mmc.d \
-./src/nand.d \
-./src/nor.d \
-./src/pcap.d \
-./src/ps7_init.d \
-./src/qspi.d \
-./src/rsa.d \
-./src/sd.d 
-
-S_UPPER_DEPS += \
-./src/fsbl_handoff.d 
-
-
-# Each subdirectory must supply rules for building sources it contributes
-src/%.o: ../src/%.c
-	@echo 'Building file: $<'
-	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -Wall -O2 -c -fmessage-length=0 -I../../zybo_fsbl_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
-	@echo 'Finished building: $<'
-	@echo ' '
-
-src/%.o: ../src/%.S
-	@echo 'Building file: $<'
-	@echo 'Invoking: ARM gcc compiler'
-	arm-xilinx-eabi-gcc -Wall -O2 -c -fmessage-length=0 -I../../zybo_fsbl_bsp/ps7_cortexa9_0/include -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
-	@echo 'Finished building: $<'
-	@echo ' '
-
-
diff --git a/quad/xsdk_workspace/zybo_fsbl_bsp/.cproject b/quad/xsdk_workspace/zybo_fsbl_bsp/.cproject
index 5129f789af122de2e3a649f495991df8ea4f0b4e..2ed788b9c962e8616019e7ed814e42f7ac4936cf 100644
--- a/quad/xsdk_workspace/zybo_fsbl_bsp/.cproject
+++ b/quad/xsdk_workspace/zybo_fsbl_bsp/.cproject
@@ -3,8 +3,8 @@
 
 <cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
 	<storageModule moduleId="org.eclipse.cdt.core.settings">
-		<cconfiguration id="org.eclipse.cdt.core.default.config.1389527391">
-			<storageModule buildSystemId="org.eclipse.cdt.core.defaultConfigDataProvider" id="org.eclipse.cdt.core.default.config.1389527391" moduleId="org.eclipse.cdt.core.settings" name="Configuration">
+		<cconfiguration id="org.eclipse.cdt.core.default.config.1193087237">
+			<storageModule buildSystemId="org.eclipse.cdt.core.defaultConfigDataProvider" id="org.eclipse.cdt.core.default.config.1193087237" moduleId="org.eclipse.cdt.core.settings" name="Configuration">
 				<externalSettings/>
 				<extensions/>
 			</storageModule>