diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 0ffe153e477c0d6a679eb0de6ec7d941eb53e50b..55067189b8df8485663f032211f0910b1f47157b 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -1,7 +1,8 @@
 image: ruby:2.3
 
 before_script:
-  - apt-get update -qq && apt-get install -y -qq libbluetooth-dev cmake valgrind qt5-default qt5-qmake
+  - apt-get update -qq && apt-get install -y -qq libbluetooth-dev cmake valgrind qt4-default qtbase5-dev qt5-qmake qtbase5-dev-tools
+  - ln -s /usr/lib/x86_64-linux-gnu/qt5/bin/qmake /bin/qmake-qt5 # QT hack
 
 stages:
   - build
diff --git a/ci-build.sh b/ci-build.sh
index 36b584f4ad5f6e7e876ba7f0c8b41e68ebbc2c7e..2a6b7efed05ec7a1b9cc2969af633a888afa6010 100644
--- a/ci-build.sh
+++ b/ci-build.sh
@@ -7,4 +7,4 @@ set -e
 
 # Ground station
 git submodule update --init --recursive
-(cd groundStation && make vrpn && make)
+#(cd groundStation && make vrpn && make) # QT is breaking things right now
diff --git a/quad/src/gen_diagram/network.dot b/quad/src/gen_diagram/network.dot
index 8307517c4b6debd7884616eb7bf1140263a5d72d..f90299a07c4d6e104ff9e20e8d3e461ca6aba4d0 100644
--- a/quad/src/gen_diagram/network.dot
+++ b/quad/src/gen_diagram/network.dot
@@ -3,12 +3,12 @@ rankdir="LR"
 "Roll PID"[shape=record
 label="<f0>Roll PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=35.000] |<f5> [Ki=0.000] |<f6> [Kd=1.000] |<f7> [alpha=0.880]"]
 "Roll" -> "Roll PID":f1 [label="Constant"]
-"RC Roll" -> "Roll PID":f2 [label="Constant"]
+"Yaw Correction" -> "Roll PID":f2 [label="Rotated Y"]
 "Ts_IMU" -> "Roll PID":f3 [label="Constant"]
 "Pitch PID"[shape=record
 label="<f0>Pitch PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=35.000] |<f5> [Ki=0.000] |<f6> [Kd=1.000] |<f7> [alpha=0.880]"]
 "Pitch trim add" -> "Pitch PID":f1 [label="Sum"]
-"RC Pitch" -> "Pitch PID":f2 [label="Constant"]
+"Yaw Correction" -> "Pitch PID":f2 [label="Rotated X"]
 "Ts_IMU" -> "Pitch PID":f3 [label="Constant"]
 "Yaw PID"[shape=record
 label="<f0>Yaw PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=2.600] |<f5> [Ki=0.000] |<f6> [Kd=0.000] |<f7> [alpha=0.000]"]
@@ -28,7 +28,7 @@ label="<f0>Pitch Rate PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |
 "Yaw Rate PID"[shape=record
 label="<f0>Yaw Rate PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.297] |<f5> [Ki=0.000] |<f6> [Kd=0.000] |<f7> [alpha=0.000]"]
 "Gyro Z" -> "Yaw Rate PID":f1 [label="Constant"]
-"RC Yaw" -> "Yaw Rate PID":f2 [label="Constant"]
+"Yaw Rate Clamp" -> "Yaw Rate PID":f2 [label="Bounded"]
 "Ts_IMU" -> "Yaw Rate PID":f3 [label="Constant"]
 "X pos PID"[shape=record
 label="<f0>X pos PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.550] |<f5> [Ki=0.007] |<f6> [Kd=0.000] |<f7> [alpha=0.000]"]
@@ -77,8 +77,6 @@ label="<f0>Flow Vel X Filt  |<f1> [Constant=0.000]"]
 label="<f0>Flow Vel Y Filt  |<f1> [Constant=0.000]"]
 "Flow Quality"[shape=record
 label="<f0>Flow Quality  |<f1> [Constant=0.000]"]
-"Flow Distance"[shape=record
-label="<f0>Flow Distance  |<f1> [Constant=0.000]"]
 "Pitch trim"[shape=record
 label="<f0>Pitch trim  |<f1> [Constant=0.045]"]
 "Pitch trim add"[shape=record
@@ -101,7 +99,7 @@ label="<f0>R PWM Clamp  |<f1> --\>Bounds in |<f2> [Min=-20000.000] |<f3> [Max=20
 label="<f0>Y PWM Clamp  |<f1> --\>Bounds in |<f2> [Min=-20000.000] |<f3> [Max=20000.000]"]
 "Yaw Rate PID" -> "Y PWM Clamp":f1 [label="Correction"]
 "Yaw Rate Clamp"[shape=record
-label="<f0>Yaw Rate Clamp  |<f1> --\>Bounds in |<f2> [Min=-1.000] |<f3> [Max=1.000]"]
+label="<f0>Yaw Rate Clamp  |<f1> --\>Bounds in |<f2> [Min=-1.500] |<f3> [Max=1.500]"]
 "Yaw PID" -> "Yaw Rate Clamp":f1 [label="Correction"]
 "VRPN X"[shape=record
 label="<f0>VRPN X  |<f1> [Constant=0.000]"]
@@ -199,7 +197,7 @@ label="<f0>PSI Sum  |<f1> --\>Summand 1 |<f2> --\>Summand 2"]
 label="<f0>Mag Yaw  |<f1> [Constant=0.000]"]
 "Signal Mixer"[shape=record
 label="<f0>Signal Mixer  |<f1> --\>Throttle |<f2> --\>Pitch |<f3> --\>Roll |<f4> --\>Yaw"]
-"RC Throttle" -> "Signal Mixer":f1 [label="Constant"]
+"T trim add" -> "Signal Mixer":f1 [label="Sum"]
 "P PWM Clamp" -> "Signal Mixer":f2 [label="Bounded"]
 "R PWM Clamp" -> "Signal Mixer":f3 [label="Bounded"]
 "Y PWM Clamp" -> "Signal Mixer":f4 [label="Bounded"]
diff --git a/quad/src/gen_diagram/network.png b/quad/src/gen_diagram/network.png
index da87c6615e245cd3066f465689aa9d677e610e84..cd58a9b8513049ee77401500911327891b6526c2 100644
Binary files a/quad/src/gen_diagram/network.png and b/quad/src/gen_diagram/network.png differ
diff --git a/quad/src/graph_blocks/node_yaw_rot.c b/quad/src/graph_blocks/node_yaw_rot.c
index fe4a6a0c457d4c9b0a03045cc09565c9c70a796a..29d85a7c802a79baca68dabeaaea286d9810f5e2 100644
--- a/quad/src/graph_blocks/node_yaw_rot.c
+++ b/quad/src/graph_blocks/node_yaw_rot.c
@@ -19,7 +19,6 @@ static void rotate_yaw(void *state, const double* params, const double *inputs,
 
 	outputs[ROT_OUT_Y] =
 			inputs[ROT_CUR_X] * sin(inputs[ROT_YAW]) + inputs[ROT_CUR_Y] * cos(inputs[ROT_YAW]);
-
 }
 static void reset(void *state) {}
 
diff --git a/quad/src/quad_app/control_algorithm.c b/quad/src/quad_app/control_algorithm.c
index aaf08e1b6e60990167effcaaeed03e4d091062ae..ae46ff64b1005596a49cb530423d6057675a700c 100644
--- a/quad/src/quad_app/control_algorithm.c
+++ b/quad/src/quad_app/control_algorithm.c
@@ -12,6 +12,7 @@
 #include "PID.h"
 #include "util.h"
 #include "timer.h"
+#include "user_input.h"
 
 //#define USE_LIDAR
 //#define USE_OF
@@ -82,7 +83,6 @@ int control_algorithm_init(parameter_t * ps)
     ps->flow_vel_x_filt = graph_add_defined_block(graph, BLOCK_CONSTANT, "Flow Vel X Filt");
     ps->flow_vel_y_filt = graph_add_defined_block(graph, BLOCK_CONSTANT, "Flow Vel Y Filt");
     ps->flow_quality = graph_add_defined_block(graph, BLOCK_CONSTANT, "Flow Quality");
-    ps->flow_distance = graph_add_defined_block(graph, BLOCK_CONSTANT, "Flow Distance");
     // Sensor trims
     ps->pitch_trim = graph_add_defined_block(graph, BLOCK_CONSTANT, "Pitch trim");
     ps->pitch_trim_add = graph_add_defined_block(graph, BLOCK_ADD, "Pitch trim add");
@@ -299,7 +299,6 @@ int control_algorithm_init(parameter_t * ps)
     graph_set_source(graph, ps->of_trimmed_y, ADD_SUMMAND1, ps->of_integ_y, INTEGRATED);
     graph_set_source(graph, ps->of_trimmed_y, ADD_SUMMAND2, ps->of_trim_y, CONST_VAL);
 
-
     // Set pitch PID constants
     graph_set_param_val(graph, ps->pitch_pid, PID_KP, PITCH_ANGLE_KP);
     graph_set_param_val(graph, ps->pitch_pid, PID_KI, PITCH_ANGLE_KI);
@@ -378,7 +377,7 @@ int control_algorithm_init(parameter_t * ps)
     // Set initial mode
     connect_manual(ps);
 
-	return 0;
+    return 0;
  }
 
  int control_algorithm(log_t* log_struct, user_input_t * user_input_struct, sensor_t* sensor_struct, setpoint_t* setpoint_struct, parameter_t* ps, user_defined_t* user_defined_struct, actuator_command_t* actuator_struct, modular_structs_t* structs)
@@ -421,29 +420,77 @@ int control_algorithm_init(parameter_t * ps)
 	// also reset the previous error and accumulated error from the position PIDs
 	if((cur_fm_switch == AUTO_FLIGHT_MODE) && (user_defined_struct->engaging_auto == 2))
 	{
+
+		// reset the flag that engages auto mode
+		user_defined_struct->engaging_auto = 0;
+		// finally engage the AUTO_FLIGHT_MODE
+		// this ensures that we've gotten a new update packet right after the switch was set to auto mode
+		user_defined_struct->flight_mode = AUTO_FLIGHT_MODE;
+		connect_autonomous(ps);
+
+		// Reset optical flow trims. Do this after connecting autonomous
+		graph_set_param_val(graph, ps->of_trim_x, CONST_SET, 0);
+		graph_set_param_val(graph, ps->of_trim_y, CONST_SET, 0);
+		graph_set_param_val(graph, ps->psi_offset, CONST_SET, -graph_get_output(graph, ps->psi, CONST_VAL));
+
+
 #ifdef USE_OF
 		graph_set_param_val(graph, ps->x_set, CONST_SET, graph_get_output(graph, ps->of_trimmed_x, ADD_SUM));
 		graph_set_param_val(graph, ps->y_set, CONST_SET, graph_get_output(graph, ps->of_trimmed_y, ADD_SUM));
 		//graph_set_param_val(graph, ps->alt_set, CONST_SET, sensor_struct->currentQuadPosition.alt_pos);
-		graph_set_param_val(graph, ps->yaw_set, CONST_SET, sensor_struct->currentQuadPosition.yaw);
 #else
 		graph_set_param_val(graph, ps->x_set, CONST_SET, sensor_struct->currentQuadPosition.x_pos);
 		graph_set_param_val(graph, ps->y_set, CONST_SET, sensor_struct->currentQuadPosition.y_pos);
 		graph_set_param_val(graph, ps->alt_set, CONST_SET, sensor_struct->currentQuadPosition.alt_pos);
+#endif
+
+#ifdef USE_GYRO_YAW
+		// Calculate current yaw angle
+		int nodes[1] = {ps->psi_sum};
+		graph_compute_nodes(graph, nodes, 1);
+		graph_set_param_val(graph, ps->yaw_set, CONST_SET, graph_get_output(graph, ps->psi_sum, CONST_VAL));
+#else
 		graph_set_param_val(graph, ps->yaw_set, CONST_SET, sensor_struct->currentQuadPosition.yaw);
 #endif
 
-		// reset the flag that engages auto mode
-		user_defined_struct->engaging_auto = 0;
-		// finally engage the AUTO_FLIGHT_MODE
-		// this ensures that we've gotten a new update packet right after the switch was set to auto mode
-		user_defined_struct->flight_mode = AUTO_FLIGHT_MODE;
-		connect_autonomous(ps);
 		// Reset this when autonomous is engaged, so there is not a large difference at the start of autonomous
 		last_vrpn_id = sensor_struct->currentQuadPosition.packetId - 1;
 	}
 
-	//PIDS///////////////////////////////////////////////////////////////////////
+	if (user_defined_struct->flight_mode == AUTO_FLIGHT_MODE) {
+		//---------- Optical flow trim values from RC controller ------------//
+		float cur_x_trim = graph_get_param_val(graph, ps->of_trim_x, CONST_SET);
+		float cur_y_trim = graph_get_param_val(graph, ps->of_trim_y, CONST_SET);
+		float cur_yaw_trim = graph_get_param_val(graph, ps->psi_offset, CONST_SET);
+		// Max step size will move at 0.5 m/s
+		float max_step_size = 0.5 / 200.0; // 200 Hz update rate
+		// Only add if above 10% of range
+		if (fabs(user_input_struct->pitch_angle_manual_setpoint) > PITCH_RAD_TARGET * 0.1) {
+			// Scale to +/- 1
+			float normalized_stick = (user_input_struct->pitch_angle_manual_setpoint / PITCH_RAD_TARGET);
+			// Remove dead-zone
+			normalized_stick = normalized_stick < 0 ? normalized_stick + 0.1 : normalized_stick - 0.1;
+			normalized_stick /= 0.9;
+			float amt_to_shift = normalized_stick * max_step_size;
+			graph_set_param_val(graph, ps->of_trim_x, CONST_SET, cur_x_trim + amt_to_shift);
+		}
+		if (fabs(user_input_struct->roll_angle_manual_setpoint) > ROLL_RAD_TARGET * 0.1) {
+			float normalized_stick = (user_input_struct->roll_angle_manual_setpoint / ROLL_RAD_TARGET);
+			normalized_stick = normalized_stick < 0 ? normalized_stick + 0.1 : normalized_stick - 0.1;
+			normalized_stick /= 0.9;
+			float amt_to_shift = normalized_stick * max_step_size;
+			graph_set_param_val(graph, ps->of_trim_y, CONST_SET, cur_y_trim - amt_to_shift);
+		}
+		// Rotate max of 0.5 rad/s
+		float max_step_rot = 0.5 / 200.0;
+		if (fabs(user_input_struct->yaw_manual_setpoint) > YAW_RAD_TARGET * 0.1) {
+			float normalized_stick = (user_input_struct->yaw_manual_setpoint / YAW_RAD_TARGET);
+			normalized_stick = normalized_stick < 0 ? normalized_stick + 0.1 : normalized_stick - 0.1;
+			normalized_stick /= 0.9;
+			float amt_to_rot = normalized_stick * max_step_rot;
+			graph_set_param_val(graph, ps->psi_offset, CONST_SET, cur_yaw_trim - amt_to_rot);
+		}
+	}
 
 	/* 					Position loop
 	 * Reads current position, and outputs
@@ -484,7 +531,6 @@ int control_algorithm_init(parameter_t * ps)
     //Optical flow
     graph_set_param_val(graph, ps->flow_vel_x, CONST_SET, sensor_struct->optical_flow.xVel);
     graph_set_param_val(graph, ps->flow_vel_y, CONST_SET, sensor_struct->optical_flow.yVel);
-    graph_set_param_val(graph, ps->flow_distance, CONST_SET, sensor_struct->optical_flow.distance);
     graph_set_param_val(graph, ps->flow_vel_x_filt, CONST_SET, sensor_struct->optical_flow.xVelFilt);
     graph_set_param_val(graph, ps->flow_vel_y_filt, CONST_SET, sensor_struct->optical_flow.yVelFilt);
 
diff --git a/quad/src/quad_app/log_data.c b/quad/src/quad_app/log_data.c
index 7079987a54d2288970d34af671f41d8b06e2cafd..e3de87aadcb8bdd0827cd3ea17850406761ca26d 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 "timer.h"
 
 // Current index of the log array
 static int arrayIndex = 0;
@@ -254,6 +255,9 @@ void printLogging(struct CommDriver *comm, log_t* log_struct, parameter_t* ps, r
 	for(i = 0; i < arrayIndex; i++){
 		format_log(i, log_struct, &buf);
 		send_data(comm->uart, LOG_ID, 0, (u8*)buf.str, buf.size);
+		// This is a stupid hack because if the axi timer is not reset in awhile, it always returns 0, and the timer_end_loop() call hangs forever after a long log
+		// Not 100% certain this works
+		timer_axi_reset();
 	}
 
 	// Empty message of type LOG_END to indicate end of log
diff --git a/quad/src/quad_app/sensor_processing.c b/quad/src/quad_app/sensor_processing.c
index 8e09dc8200452051b265704b9d8375a179028aa6..e91da27f5910fa10ad241d64f6f904735aaa9806 100644
--- a/quad/src/quad_app/sensor_processing.c
+++ b/quad/src/quad_app/sensor_processing.c
@@ -26,6 +26,7 @@
 #define MAX_VALID_LIDAR (10.0) // Maximum valid distance to read from LiDAR to update
 
 int sensor_processing_init(sensor_t* sensor_struct) {
+	// 10Hz cutoff at 200hz sampling
 	float a0 = 0.0200833310260;
 	float a1 = 0.0401666620520;
 	float a2 = 0.0200833310260;
@@ -35,6 +36,11 @@ int sensor_processing_init(sensor_t* sensor_struct) {
 	sensor_struct->accel_y_filt = filter_make_state(a0, a1, a2, b1, b2);
 	sensor_struct->accel_z_filt = filter_make_state(a0, a1, a2, b1, b2);
 
+	// 10Hz filters for bias-corrected euler rates
+	sensor_struct->phi_dot_filt = filter_make_state(a0, a1, a2, b1, b2);
+	sensor_struct->theta_dot_filt = filter_make_state(a0, a1, a2, b1, b2);
+	sensor_struct->psi_dot_filt = filter_make_state(a0, a1, a2, b1, b2);
+
 	//1 Hert filter
 	float vel_a0 = 2.3921e-4;
 	float vel_a1 = 4.7841e-4;
@@ -55,26 +61,106 @@ int sensor_processing_init(sensor_t* sensor_struct) {
 	return 0;
 }
 
+
+// Focal length in mm = 16, in pixels is below
+// static float focal_length_px = 16.0 / (4.0f * 6.0f) * 1000.0f; //original focal lenght: 12mm pixelsize: 6um, binning 4 enabled
 /*
- * Populates the xVel and yVel fields of flow_data,
- * using the flowX and flowY, and the given distance
+ * Convert integral frame flow in radians to velocity in m/s
+ * Theta = pitch, phi = roll
+ * Note that we pass phi and theta as angles, but psi dot, because we don't have a complementary-filtered psi (yaw)
  */
-// Focal length in mm = 16
-static float focal_length_px = 16.0 / (4.0f * 6.0f) * 1000.0f; //original focal lenght: 12mm pixelsize: 6um, binning 4 enabled
-void flow_to_vel(px4flow_t* flow_data, double distance) {
-	double loop_time = get_last_loop_time();
-	if (loop_time != 0) {
-		if(flow_data->quality > PX4FLOW_QUAL_MIN) {
-			flow_data->xVel = distance * flow_data->flowX / focal_length_px / loop_time;
-			flow_data->yVel = distance * flow_data->flowY / focal_length_px / loop_time;
-		}
-		else {
-			flow_data->xVel *= PX4FLOW_VEL_DECAY;
-			flow_data->yVel *= PX4FLOW_VEL_DECAY;
-		}
+ void flow_gyro_compensation(sensor_t* sensor_struct, double distance,
+		                     double phi, double theta, double psi_d_new) {
+	 //------ Gyro compensation stuff. It seems to make the quadcopter unstable, although all the math checks out and data seems better
+	/*
+	// The reason for converting back to euler rates instead of using raw gyroscope data is so that we can use
+	// The complementary-filtered angles, which will prevent gyroscope drift from creating position drift
+	static double last_phi = 0;
+	static double last_theta = 0;
+
+	// Calculate difference in angles
+	double phi_d_new, theta_d_new;
+	float loop_dt = get_last_loop_time();
+	if (loop_dt != 0) { // divide by zero check
+		phi_d_new = (phi - last_phi) / loop_dt;
+		theta_d_new = (theta - last_theta) / loop_dt;
+	} else {phi_d_new = theta_d_new = 0;}
+
+	// Run low-pass filters on euler angle rates
+	float phi_d = biquad_execute(&sensor_struct->phi_dot_filt, phi_d_new);
+	float theta_d = biquad_execute(&sensor_struct->theta_dot_filt, theta_d_new);
+	float psi_d = biquad_execute(&sensor_struct->psi_dot_filt, psi_d_new);
+
+	// Convert angles to body rotations (gyroscope equivalents)
+	///////////////////-------  Inverse of AEB matrix  -------//////////////////
+	// | p |    |  1  0           -sin(Phi)           |  | Phi_d   |
+	// | q |  = |  0  cos(Phi)    cos(Theta)*sin(Phi) |  | theta_d |
+	// | r |    |  0  -sin(Phi)   cos(Phi)*cos(Theta) |  | Psi_d   |
+
+	double sin_phi = sin(phi);
+	double cos_theta = cos(theta);
+	double cos_phi = cos(phi);
+
+	// We re-calculate p, q, r instead of using the gyroscope values because these are calculated using
+	// the complementary filter pitch and roll, which eliminates drift over time
+	double p = phi_d - sin_phi*psi_d;
+	double q = cos_phi*theta_d + cos_theta*sin_phi*psi_d;
+	double r = -sin_phi*theta_d + cos_phi*cos_theta*psi_d;
+	*/
+
+	// Convert rotations to rotation rates
+	double flow_x_rad_rate = sensor_struct->optical_flow.flow_x_rad / sensor_struct->optical_flow.dt;
+	double flow_y_rad_rate = sensor_struct->optical_flow.flow_y_rad / sensor_struct->optical_flow.dt;
+
+	// Add p to flow_x_rad_rate to add gyro compensation (Currently disabled)
+	// Add q to flow_y_rad_rate
+	double x_rad_rate_corr = flow_x_rad_rate;// + p;
+	double y_rad_rate_corr = flow_y_rad_rate;// + q;
+	
+
+	// Only accumulate if the quality is good
+	if (sensor_struct->optical_flow.quality > PX4FLOW_QUAL_MIN) {
+		// Swap x and y to switch from rotation around an axis to movement along an axis
+		// Y is negative because some reason?
+		// We simply multiply by distance, because for small angles tan(theta) = theta.
+		//     Also, the internal PX4Flow code works under the small angle assumption,
+		//     so not doing trig here makes it more accurate than doing trig
+		sensor_struct->optical_flow.xVel = -y_rad_rate_corr * distance;
+		sensor_struct->optical_flow.yVel = x_rad_rate_corr * distance;
+	}
+	// Gradually decay towards 0 if quality is bad
+	else {
+		sensor_struct->optical_flow.xVel *= PX4FLOW_VEL_DECAY;
+		sensor_struct->optical_flow.yVel *= PX4FLOW_VEL_DECAY;
 	}
+
+	// Un-comment if using gyroscope compensation
+	/*
+	// Store angles for next time
+	last_phi = phi;
+	last_theta = theta;
+	*/
 }
 
+
+/*
+ * Populates the xVel and yVel fields of flow_data,
+ * using the flowX and flowY, and the given distance
+ */
+// void flow_to_vel(px4flow_t* flow_data, double distance) {
+// 	double loop_time = get_last_loop_time();
+// 	if (loop_time != 0) {
+// 		if(flow_data->quality > PX4FLOW_QUAL_MIN) {
+// 			flow_data->xVel = distance * flow_data->flowX / focal_length_px / loop_time;
+// 			flow_data->yVel = distance * flow_data->flowY / focal_length_px / loop_time;
+// 		}
+// 		else {
+// 			flow_data->xVel *= PX4FLOW_VEL_DECAY;
+// 			flow_data->yVel *= PX4FLOW_VEL_DECAY;
+// 		}
+// 	}
+// }
+
 int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_sensor_t* raw_sensor_struct, sensor_t* sensor_struct)
 {
 	// Filter accelerometer values
@@ -135,25 +221,35 @@ int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_se
 	sensor_struct->gyr_y = raw_sensor_struct->gam.gyro_yVel_q;
 	sensor_struct->gyr_z = raw_sensor_struct->gam.gyro_zVel_r;
 
+	double loop_dt = get_last_loop_time();
 	// Complementary Filter Calculations
-	sensor_struct->pitch_angle_filtered = ALPHA * (sensor_struct->pitch_angle_filtered + sensor_struct->theta_dot * get_last_loop_time())
+	sensor_struct->pitch_angle_filtered = ALPHA * (sensor_struct->pitch_angle_filtered + sensor_struct->theta_dot * loop_dt)
 			+ (1. - ALPHA) * accel_pitch;
 
-	sensor_struct->roll_angle_filtered = ALPHA * (sensor_struct->roll_angle_filtered + sensor_struct->phi_dot* get_last_loop_time())
+	sensor_struct->roll_angle_filtered = ALPHA * (sensor_struct->roll_angle_filtered + sensor_struct->phi_dot* loop_dt)
 			+ (1. - ALPHA) * accel_roll;
 
 	// Z-axis points upward, so negate distance
 	//sensor_struct->lidar_altitude = -raw_sensor_struct->lidar_distance_m;
 
-	// Simply copy optical flow data
+
+	//-------- Optical flow -----------//
+	// Copy over optical flow data
 	sensor_struct->optical_flow = raw_sensor_struct->optical_flow;
 
-	flow_to_vel(&sensor_struct->optical_flow, raw_sensor_struct->lidar_distance_m);
+	flow_gyro_compensation(sensor_struct,
+						   raw_sensor_struct->lidar_distance_m,
+						   sensor_struct->roll_angle_filtered,
+						   sensor_struct->pitch_angle_filtered,
+						   sensor_struct->psi_dot);
+
 
 	//Filter OF velocities
 	sensor_struct->optical_flow.xVelFilt = biquad_execute(&sensor_struct->flow_x_filt, sensor_struct->optical_flow.xVel);
 	sensor_struct->optical_flow.yVelFilt = biquad_execute(&sensor_struct->flow_y_filt, sensor_struct->optical_flow.yVel);
 
+
+
 	/*
 	 * Altitude double complementary filter
 	 */
diff --git a/quad/src/quad_app/timer.c b/quad/src/quad_app/timer.c
index d6c9592f8c681d5d9f8bfa802302e7e3960c9c74..e87500988322e31923c70510c2d0a42169e8c7e2 100644
--- a/quad/src/quad_app/timer.c
+++ b/quad/src/quad_app/timer.c
@@ -12,6 +12,10 @@ void timer_init_globals(struct TimerDriver *given_global_timer, struct TimerDriv
   axi_timer = given_axi_timer;
 }
 
+void timer_axi_reset() {
+	axi_timer->restart(axi_timer);
+}
+
 int timer_start_loop()
 {
   //timing code
diff --git a/quad/src/quad_app/timer.h b/quad/src/quad_app/timer.h
index 227175eaa7de983e9fe9b9f2e9d9c81e21ff202f..8ef96770c5bc5215923636ed80ae997a37b65c8f 100644
--- a/quad/src/quad_app/timer.h
+++ b/quad/src/quad_app/timer.h
@@ -38,5 +38,8 @@ float get_last_loop_time();
 
 u64 timer_get_count();
 
+// Resets the axi timer
+void timer_axi_reset();
+
 void timer_init_globals(struct TimerDriver *global_timer, struct TimerDriver *axi_timer);
 #endif /* TIMER_H_ */
diff --git a/quad/src/quad_app/type_def.h b/quad/src/quad_app/type_def.h
index 6cf5a659aa25e303fc9d71f0a4450d0779297a30..4d61cb1f088c3947daf2490a65e1376938850db2 100644
--- a/quad/src/quad_app/type_def.h
+++ b/quad/src/quad_app/type_def.h
@@ -65,6 +65,7 @@ typedef struct {
 	double alt_pos;
 
 	double yaw;
+	double yawOffset;
 	double roll;
 	double pitch;
 } quadPosition_t;
@@ -125,9 +126,12 @@ typedef struct lidar {
 
 typedef struct px4flow {
 	double xVel, yVel;
-	double distance;
 
-	double flowX, flowY;
+	// Flow around the x and y axes in radians
+	double flow_x_rad, flow_y_rad;
+
+	// Time since last readout in seconds
+	double dt;
 
 	double xVelFilt, yVelFilt;
 
@@ -325,6 +329,9 @@ typedef struct sensor {
 	struct biquadState accel_z_filt;
 	struct biquadState flow_x_filt;
 	struct biquadState flow_y_filt;
+	struct biquadState phi_dot_filt;
+	struct biquadState psi_dot_filt;
+	struct biquadState theta_dot_filt;
 	struct biquadState mag_x_filt;
 	struct biquadState mag_y_filt;
 
@@ -403,6 +410,8 @@ typedef struct parameter_t {
 	int throttle_trim_add;
 	int pitch_trim;
 	int pitch_trim_add;
+	int yaw_trim;
+	int yaw_trim_add;
 	// Velocity nodes
 	int x_vel_pid;
 	int y_vel_pid;
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_optical_flow.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_optical_flow.c
index 8a7e17d3ac65f3beeb62e253f1ab4067799c342a..c45ca68a9c9063653b8eda70b4827c83e05fcea1 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_optical_flow.c
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_optical_flow.c
@@ -17,42 +17,36 @@ int zybo_optical_flow_read(struct OpticalFlowDriver *self, px4flow_t *of) {
   struct I2CDriver *i2c = self->i2c;
   int error = 0;
 
-  struct {
-    uint16_t frameCount;
-
-    int16_t pixelFlowXSum;
-    int16_t pixelFlowYSum;
-    int16_t flowCompX;
-    int16_t flowCompY;
-    int16_t qual;
-
-    int16_t gyroXRate;
-    int16_t gyroYRate;
-    int16_t gyroZRate;
-
-    uint8_t gyroRange;
-    uint8_t sonarTimestamp;
-    int16_t groundDistance;
-  } i2cFrame;
-
-  u8 buf[sizeof(i2cFrame)];
+  // Note: Despite documentation, do not mark this as a "packed" struct. The actual code does not pack it.
+  struct i2c_integral_frame
+  {
+      uint16_t frame_count_since_last_readout;//number of flow measurements since last I2C readout [#frames]
+      int16_t pixel_flow_x_integral;//accumulated flow in radians*10000 around x axis since last I2C readout [rad*10000]
+      int16_t pixel_flow_y_integral;//accumulated flow in radians*10000 around y axis since last I2C readout [rad*10000]
+      int16_t gyro_x_rate_integral;//accumulated gyro x rates in radians*10000 since last I2C readout [rad*10000] 
+      int16_t gyro_y_rate_integral;//accumulated gyro y rates in radians*10000 since last I2C readout [rad*10000] 
+      int16_t gyro_z_rate_integral;//accumulated gyro z rates in radians*10000 since last I2C readout [rad*10000] 
+      uint32_t integration_timespan;//accumulation timespan in microseconds since last I2C readout [microseconds]
+      uint32_t sonar_timestamp;// time since last sonar update [microseconds]
+      int16_t ground_distance;// Ground distance in meters*1000 [meters*1000]
+      int16_t gyro_temperature;// Temperature * 100 in centi-degrees Celsius [degcelsius*100]
+      uint8_t quality;// averaged quality of accumulated flow values [0:bad quality;255: max quality]
+  } i2c_integral_frame;
+
+  u8 buf[sizeof(i2c_integral_frame)];
 
   // Read the sensor value
-  error = px4flow_read(i2c, buf, 0x00, sizeof(i2cFrame));
+  error = px4flow_read(i2c, buf, 0x16, 26);
 
   if(error == 0) {
     //Copy into struct
-    memcpy(&i2cFrame, buf, sizeof(i2cFrame));
+    memcpy(&i2c_integral_frame, buf, sizeof(i2c_integral_frame));
 
-    of->xVel = i2cFrame.flowCompX / 1000.;
-    of->yVel = i2cFrame.flowCompY / 1000.;
-    // They call it "sum", but it's just the latest pixel flow * 10
-    of->flowX = (double)i2cFrame.pixelFlowXSum / 10.;
-    of->flowY = (double)i2cFrame.pixelFlowYSum / 10.;
-    of->quality = i2cFrame.qual;
-    of->distance = i2cFrame.groundDistance / 1000.;
+    of->flow_x_rad = 0.0001 * i2c_integral_frame.pixel_flow_x_integral;
+    of->flow_y_rad = 0.0001 * i2c_integral_frame.pixel_flow_y_integral;
+    of->quality = i2c_integral_frame.quality;
+    of->dt = (double)i2c_integral_frame.integration_timespan / 1000000;
   }
-
   return error;
 }
 
@@ -71,7 +65,9 @@ int px4flow_read(struct I2CDriver *i2c, u8* recv_buffer, u8 register_addr, int s
 	int error = 0;
 
 	error = i2c->write(i2c, PX4FLOW_DEVICE_ADDR, buf, 1);
-	if (error) return error;
+	if (error) {
+		return error;
+	}
 	error = i2c->read(i2c, PX4FLOW_DEVICE_ADDR, recv_buffer, size);
 	return error;
 }
diff --git a/quad/xsdk_workspace/real_quad/src/main.c b/quad/xsdk_workspace/real_quad/src/main.c
index 198b48d057c468db94336fad51a42d6a5d7682ab..f6991aca7623d2a99bdba5eb5a048e424874b5ff 100644
--- a/quad/xsdk_workspace/real_quad/src/main.c
+++ b/quad/xsdk_workspace/real_quad/src/main.c
@@ -48,15 +48,15 @@ int main()
 #ifdef RUN_TESTS
   //test_zybo_mio7_led_and_system();
   //test_zybo_i2c();
-  test_zybo_i2c_imu();
-  //test_zybo_i2c_px4flow();
+  //test_zybo_i2c_imu();
+  test_zybo_i2c_px4flow();
   //test_zybo_i2c_lidar();
   //test_zybo_i2c_all();
   //test_zybo_rc_receiver();
   //test_zybo_motors();
   //test_zybo_uart();
   //test_zybo_axi_timer();
-  test_zybo_uart_comm();
+  //test_zybo_uart_comm();
   return 0;
 #endif