diff --git a/quad/src/quad_app/control_algorithm.c b/quad/src/quad_app/control_algorithm.c
index aaf08e1b6e60990167effcaaeed03e4d091062ae..00a1fd39e993ed354292ec5bfc03b66860359107 100644
--- a/quad/src/quad_app/control_algorithm.c
+++ b/quad/src/quad_app/control_algorithm.c
@@ -82,7 +82,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");
@@ -484,7 +483,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/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/type_def.h b/quad/src/quad_app/type_def.h
index 6cf5a659aa25e303fc9d71f0a4450d0779297a30..e6e7d93f07ec966d525641cc1d75a759251fc3f1 100644
--- a/quad/src/quad_app/type_def.h
+++ b/quad/src/quad_app/type_def.h
@@ -125,9 +125,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 +328,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;
 
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