diff --git a/quad/src/quad_app/control_algorithm.c b/quad/src/quad_app/control_algorithm.c
index bd21410cdf568da41a23a4a1b5207b612bcf4d36..692807713e33760de2c7d559839d05cfe057616e 100644
--- a/quad/src/quad_app/control_algorithm.c
+++ b/quad/src/quad_app/control_algorithm.c
@@ -17,7 +17,7 @@
 #define MAX_VALID_LIDAR (10.0) // Maximum valid distance to read from LiDAR to update
 
 #define PX4FLOW_QUAL_MIN			(100)
-#define OF_OFFSET_ANGLE             (-0.62204) // -35.64 degrees
+#define OF_OFFSET_ANGLE             (0.62204) // 35.64 degrees
 
 #define ROLL_PITCH_MAX_ANGLE 0.35 // 20 degrees
 #define PWM_DIFF_BOUNDS 20000
@@ -416,8 +416,8 @@ int control_algorithm_init(parameter_t * ps)
     graph_set_param_val(graph, ps->rc_throttle, CONST_SET, user_input_struct->rc_commands[THROTTLE]);
 
     // Compute VRPN blocks so they can be logged
-    int outputs[1] = {ps->mixer};
-    graph_compute_nodes(graph, outputs, 1);
+    int outputs[3] = {ps->mixer, ps->of_integ_x, ps->of_integ_y};
+    graph_compute_nodes(graph, outputs, 3);
 
 	 // here for now so in case any flight command is not PID controlled, it will default to rc_command value:
 	//memcpy(raw_actuator_struct->controller_corrected_motor_commands, user_input_struct->rc_commands, sizeof(int) * 6);
diff --git a/quad/src/quad_app/iic_utils.c b/quad/src/quad_app/iic_utils.c
index dffe2f2313b956a413eb86da40e6839fe5c30eb6..5674ac93bb4cd6c3ea202181a1c702c75015164e 100644
--- a/quad/src/quad_app/iic_utils.c
+++ b/quad/src/quad_app/iic_utils.c
@@ -64,7 +64,7 @@ void iic0_mpu9150_stop(){
 	iic0_mpu9150_write(0x6B, 0b01000000);
 }
 
-void iic0_mpu9150_write(u8 register_addr, u8 data){
+int iic0_mpu9150_write(u8 register_addr, u8 data){
 
 	u16 device_addr = MPU9150_DEVICE_ADDR;
 	u8 buf[] = {register_addr, data};
@@ -78,10 +78,10 @@ void iic0_mpu9150_write(u8 register_addr, u8 data){
 		device_addr = MPU9150_COMPASS_ADDR;
 	}
 
-	i2c->write(i2c, device_addr, buf, 2);
+	return i2c->write(i2c, device_addr, buf, 2);
 }
 
-void iic0_mpu9150_read(u8* recv_buffer, u8 register_addr, int size){
+int iic0_mpu9150_read(u8* recv_buffer, u8 register_addr, int size){
 
 	u16 device_addr = MPU9150_DEVICE_ADDR;
 	u8 buf[] = {register_addr};
@@ -96,8 +96,11 @@ void iic0_mpu9150_read(u8* recv_buffer, u8 register_addr, int size){
 	}
 
 
-	i2c->write(i2c, device_addr, buf, 1);
-	i2c->read(i2c, device_addr, recv_buffer, size);
+	int status = i2c->write(i2c, device_addr, buf, 1);
+	if (!status) {
+		status = i2c->read(i2c, device_addr, recv_buffer, size);
+	}
+	return status;
 }
 
 void iic0_mpu9150_calc_mag_sensitivity(){
@@ -156,6 +159,15 @@ void iic0_mpu9150_read_mag(gam_t* gam){
 
 }
 
+static int iic_rx_timeout_failures;
+int iic_get_gam_timeout_failures() {
+	return iic_rx_timeout_failures;
+}
+static int iic_cons_timeout_failures;
+int iic_get_consecutive_gam_timeout_failures() {
+	return iic_cons_timeout_failures;
+}
+
 /**
  * Get Gyro Accel Mag (GAM) information
  */
@@ -170,7 +182,15 @@ int iic0_mpu9150_read_gam(gam_t* gam) {
 	//Xint8 mag_data[6] = {};
 
 	//readHandler = iic0_read_bytes(sensor_data, ACCEL_GYRO_BASE_ADDR, ACCEL_GYRO_READ_SIZE);
-	iic0_mpu9150_read(sensor_data, ACCEL_GYRO_BASE_ADDR, ACCEL_GYRO_READ_SIZE);
+	int status = iic0_mpu9150_read(sensor_data, ACCEL_GYRO_BASE_ADDR, ACCEL_GYRO_READ_SIZE);
+	if (status == IIC_RX_TIMEOUT_FAILURE) {
+		iic_rx_timeout_failures++;
+		iic_cons_timeout_failures++;
+		return status;
+	} else if (status) {
+		return status;
+	}
+	iic_cons_timeout_failures = 0; // Successful read
 
 	//Calculate accelerometer data
 	raw_accel_x = sensor_data[ACC_X_H] << 8 | sensor_data[ACC_X_L];
diff --git a/quad/src/quad_app/iic_utils.h b/quad/src/quad_app/iic_utils.h
index 59ad8ed7c73580004717c6c9f4eaaffe14f3be5e..c7f4bc0b8ee38b38620c98b1ee10ab57cf95f8db 100644
--- a/quad/src/quad_app/iic_utils.h
+++ b/quad/src/quad_app/iic_utils.h
@@ -46,6 +46,9 @@
 #define WRITE_INTR_MASK (ARB_LOST | TIME_OUT | RX_OVF | TX_OVF | NACK)
 #define READ_INTR_MASK (ARB_LOST | TIME_OUT | RX_OVF | RX_UNF | NACK)
 
+// Error code could be returned on an iic read if the watchdog timer triggers
+#define IIC_RX_TIMEOUT_FAILURE (-88)
+
 ///////////////////////////////////////////////////////////////////////////////
 // MPU9150 Sensor Defines (Address is defined on the Sparkfun MPU9150 Datasheet)
 ///////////////////////////////////////////////////////////////////////////////
@@ -98,8 +101,8 @@
 
 void iic_set_globals(struct I2CDriver *given_i2c, struct SystemDriver *given_system);
 
-void iic0_mpu9150_write(u8 register_addr, u8 data);
-void iic0_mpu9150_read(u8* recv_buffer, u8 register_addr, int size);
+int iic0_mpu9150_write(u8 register_addr, u8 data);
+int iic0_mpu9150_read(u8* recv_buffer, u8 register_addr, int size);
 
 // Wake up the MPU for data collection
 // Configure Gyro/Accel/Mag
@@ -112,6 +115,8 @@ void iic0_mpu9150_calc_mag_sensitivity();
 void iic0_mpu9150_read_mag(gam_t* gam);
 void iic0_mpu9150_read_gyro_accel(gam_t* gam);
 
+int iic_get_gam_timeout_failures();
+int iic_get_consecutive_gam_timeout_failures();
 int iic0_mpu9150_read_gam(gam_t* gam);
 
 ////////////////////////////////////////////////////////////////////////////////////////////
diff --git a/quad/src/quad_app/log_data.c b/quad/src/quad_app/log_data.c
index 92c575f56e79e79355b818e1f00419a0925a6113..e83dce486edd0851ae952ec614e4b863bb85bd21 100644
--- a/quad/src/quad_app/log_data.c
+++ b/quad/src/quad_app/log_data.c
@@ -15,6 +15,7 @@
 #include "communication.h"
 #include "computation_graph.h"
 #include "graph_blocks.h"
+#include "iic_utils.h"
 
 // Current index of the log array
 static int arrayIndex = 0;
@@ -203,6 +204,7 @@ void printLogging(hardware_t *hardware_struct, log_t* log_struct, parameter_t* p
 	int i;
 	// Comment header
 	safe_sprintf_cat(&buf, "# MicroCART On-board Quad Log\n# Sample size: %d\n", arrayIndex);
+	safe_sprintf_cat(&buf, "# IIC failures: %d\n", iic_get_gam_timeout_failures());
 
 	// List Pid Constants
 	for (i = 0; i < ps->graph->n_nodes; ++i) {
diff --git a/quad/src/quad_app/quad_app.c b/quad/src/quad_app/quad_app.c
index f83cc8698f58c41a42ae255808f1c749b442bdbb..a8a81a08341b4fac5a58892a10a73036bba6ded2 100644
--- a/quad/src/quad_app/quad_app.c
+++ b/quad/src/quad_app/quad_app.c
@@ -42,6 +42,7 @@ int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
 	// Main control loop
 	while (1)
 	{
+		// Kill quad if kill switch or more than 10 GAM
 		int this_kill_condition = kill_condition(&(structs.user_input_struct));
 
 		// Processing of loop timer at the beginning of the control loop
@@ -112,6 +113,12 @@ int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
 
 		// Processing of loop timer at the end of the control loop
 		timer_end_loop(&(structs.log_struct));
+
+		if (iic_get_consecutive_gam_timeout_failures() > 10) {
+			kill_motors(&(structs.hardware_struct.pwm_outputs));
+			printLogging(&structs.hardware_struct, &(structs.log_struct), &(structs.parameter_struct));
+			break;
+		}
 	}
 
 	kill_motors(&(structs.hardware_struct.motors));
diff --git a/quad/src/quad_app/sensor_processing.c b/quad/src/quad_app/sensor_processing.c
index 668afa23fc6d16d4056d6b118ca5674c8a62f78e..e6ba1660cf445869a5f813a8ec3c9bc893f6603f 100644
--- a/quad/src/quad_app/sensor_processing.c
+++ b/quad/src/quad_app/sensor_processing.c
@@ -101,6 +101,8 @@ int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_se
 
 	// Simply copy optical flow data
 	sensor_struct->optical_flow = raw_sensor_struct->optical_flow;
+	sensor_struct->optical_flow.xVel = -sensor_struct->optical_flow.xVel;
+	sensor_struct->optical_flow.yVel = -sensor_struct->optical_flow.yVel;
 	return 0;
 }
 
diff --git a/quad/src/quad_app/timer.c b/quad/src/quad_app/timer.c
index b7f68afbc3455758702f5f55a59623af987e1dc3..d6c9592f8c681d5d9f8bfa802302e7e3960c9c74 100644
--- a/quad/src/quad_app/timer.c
+++ b/quad/src/quad_app/timer.c
@@ -47,7 +47,7 @@ float get_last_loop_time() {
   return (float)LOOP_TIME / 1000000;
 }
 
-u32 timer_get_count() {
+u64 timer_get_count() {
   u64 time;
   return axi_timer->read(axi_timer, &time);
   return time;
diff --git a/quad/src/quad_app/timer.h b/quad/src/quad_app/timer.h
index 8f3d12fb4930cf172c8747aaeaaf6a5f70a6491d..227175eaa7de983e9fe9b9f2e9d9c81e21ff202f 100644
--- a/quad/src/quad_app/timer.h
+++ b/quad/src/quad_app/timer.h
@@ -36,7 +36,7 @@ int timer_end_loop(log_t *log_struct);
 // Returns the number of seconds the last loop took
 float get_last_loop_time();
 
-u32 timer_get_count();
+u64 timer_get_count();
 
 void timer_init_globals(struct TimerDriver *global_timer, struct TimerDriver *axi_timer);
 #endif /* TIMER_H_ */
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 7748962c3f12530595b6664d64e3a712c9a9993e..a2d662cb61dd0666be88d414949fbdc4c5adda8b 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
@@ -1,6 +1,7 @@
 #include "hw_impl_zybo.h"
 #include "iic_utils.h"
 #include "xiicps.h"
+#include "timer.h"
 
 #define IO_CLK_CONTROL_REG_ADDR	(0xF800012C)
 
@@ -266,12 +267,26 @@ int XIicPs_MasterRecvPolled_ours(XIicPs *InstancePtr, u8 *MsgPtr,
 			 ByteCount);
 	}
 
+/* <--- MicroCART additions (iic watchdog timer hack) ---> */
+	u32 iic_freq = XIicPs_GetSClk(InstancePtr);
+	// (1000000 * 9 / iic_freq) is the number of microseconds required to send 1 byte of data
+	// Using 5 times as an upper bound
+	u32 max_usec_per_byte = 5 * 1000000 * 9 / iic_freq;
+	u64 start_time = timer_get_count();
+/* <--- End hack ---> */
+
 	/*
 	 * Pull the interrupt status register to find the errors.
 	 */
 	IntrStatusReg = XIicPs_ReadReg(BaseAddr, XIICPS_ISR_OFFSET);
 	while ((InstancePtr->RecvByteCount > 0) &&
 			((IntrStatusReg & Intrs) == 0) && !(IntrStatusReg & XIICPS_IXR_COMP_MASK)) {
+/* <--- MicroCART additions (iic watchdog timer hack) ---> */
+		u64 usec_passed = timer_get_count() - start_time;
+		if (usec_passed > max_usec_per_byte * (ByteCount - InstancePtr->RecvByteCount)) {
+			return IIC_RX_TIMEOUT_FAILURE;
+		}
+/* <--- End hack ---> */
 		StatusReg = XIicPs_ReadReg(BaseAddr, XIICPS_SR_OFFSET);
 
 		/*
diff --git a/quad/xsdk_workspace/real_quad/src/main.c b/quad/xsdk_workspace/real_quad/src/main.c
index 5892393e53137ad97d0c735c77d768c8f3a70d23..6eeb6ce03ce9615daaee02626ea1fdbab0a216d1 100644
--- a/quad/xsdk_workspace/real_quad/src/main.c
+++ b/quad/xsdk_workspace/real_quad/src/main.c
@@ -39,5 +39,11 @@ int main()
 
   // Run the main quad application
   quad_main(setup_hardware);
+
+  while(1) {
+	  volatile int i = 0;
+	  i++;
+  }
+
   return 0;
 }