diff --git a/quad/sw/modular_quad_pid/gen_diagram/create_png.sh b/quad/sw/modular_quad_pid/gen_diagram/create_png.sh
old mode 100644
new mode 100755
index c431622d50d7ea3c0b6a5673896d8eea1bec5cef..3389781bf0d69d913c2b8d10e055ff2945bc3846
--- a/quad/sw/modular_quad_pid/gen_diagram/create_png.sh
+++ b/quad/sw/modular_quad_pid/gen_diagram/create_png.sh
@@ -1,4 +1,7 @@
 #/bin/bash
 
+make
+./gen_diagram
+
 dot -Tpng network.dot -o network.png
 
diff --git a/quad/sw/modular_quad_pid/gen_diagram/gen_diagram b/quad/sw/modular_quad_pid/gen_diagram/gen_diagram
index 67f69853389aab3b2ae03822ea66932ef2ef902e..82a727a63fa1c424090b886e02feadd41d867e9b 100755
Binary files a/quad/sw/modular_quad_pid/gen_diagram/gen_diagram and b/quad/sw/modular_quad_pid/gen_diagram/gen_diagram differ
diff --git a/quad/sw/modular_quad_pid/gen_diagram/network.dot b/quad/sw/modular_quad_pid/gen_diagram/network.dot
index 711d7dd16753ba44337053c960bb6d0f8fc15912..14f0e486829eeaf7d39a17e9afadcf5323e533ad 100644
--- a/quad/sw/modular_quad_pid/gen_diagram/network.dot
+++ b/quad/sw/modular_quad_pid/gen_diagram/network.dot
@@ -2,49 +2,49 @@ digraph G {
 rankdir="LR"
 "Roll PID"[shape=record
 label="<f0>Roll PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=15.000] |<f5> [Ki=0.000] |<f6> [Kd=0.200]"]
-"Roll" -> "Roll PID":f1 [label="Constant"]
+"VRPN Roll" -> "Roll PID":f1 [label="Constant"]
 "RC Roll" -> "Roll PID":f2 [label="Constant"]
-"Ts_angle" -> "Roll PID":f3 [label="Constant"]
+"Ts_VRPN" -> "Roll PID":f3 [label="Constant"]
 "Pitch PID"[shape=record
 label="<f0>Pitch PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=15.000] |<f5> [Ki=0.000] |<f6> [Kd=0.200]"]
-"Pitch" -> "Pitch PID":f1 [label="Constant"]
+"VRPN Pitch" -> "Pitch PID":f1 [label="Constant"]
 "RC Pitch" -> "Pitch PID":f2 [label="Constant"]
-"Ts_angle" -> "Pitch PID":f3 [label="Constant"]
+"Ts_VRPN" -> "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]"]
 "Yaw" -> "Yaw PID":f1 [label="Constant"]
 "Yaw Setpoint" -> "Yaw PID":f2 [label="Constant"]
-"Ts_angle" -> "Yaw PID":f3 [label="Constant"]
+"Ts_IMU" -> "Yaw PID":f3 [label="Constant"]
 "Roll Rate PID"[shape=record
 label="<f0>Roll Rate PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=4600.000] |<f5> [Ki=0.000] |<f6> [Kd=550.000]"]
 "dPhi" -> "Roll Rate PID":f1 [label="Constant"]
 "Roll PID" -> "Roll Rate PID":f2 [label="Correction"]
-"Ts_angle" -> "Roll Rate PID":f3 [label="Constant"]
+"Ts_IMU" -> "Roll Rate PID":f3 [label="Constant"]
 "Pitch Rate PID"[shape=record
 label="<f0>Pitch Rate PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=4600.000] |<f5> [Ki=0.000] |<f6> [Kd=550.000]"]
 "dTheta" -> "Pitch Rate PID":f1 [label="Constant"]
 "Pitch PID" -> "Pitch Rate PID":f2 [label="Correction"]
-"Ts_angle" -> "Pitch Rate PID":f3 [label="Constant"]
+"Ts_IMU" -> "Pitch Rate PID":f3 [label="Constant"]
 "Yaw Rate PID"[shape=record
 label="<f0>Yaw Rate PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=435480.000] |<f5> [Ki=0.000] |<f6> [Kd=0.000]"]
 "dPsi" -> "Yaw Rate PID":f1 [label="Constant"]
 "RC Yaw" -> "Yaw Rate PID":f2 [label="Constant"]
-"Ts_angle" -> "Yaw Rate PID":f3 [label="Constant"]
+"Ts_VRPN" -> "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.015] |<f5> [Ki=-0.005] |<f6> [Kd=-0.030]"]
 "VRPN X" -> "X pos PID":f1 [label="Constant"]
 "X Setpoint" -> "X pos PID":f2 [label="Constant"]
-"Ts_position" -> "X pos PID":f3 [label="Constant"]
+"Ts_VRPN" -> "X pos PID":f3 [label="Constant"]
 "Y pos PID"[shape=record
 label="<f0>Y pos PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.015] |<f5> [Ki=0.005] |<f6> [Kd=0.030]"]
 "VRPN Y" -> "Y pos PID":f1 [label="Constant"]
 "Y Setpoint" -> "Y pos PID":f2 [label="Constant"]
-"Ts_position" -> "Y pos PID":f3 [label="Constant"]
+"Ts_VRPN" -> "Y pos PID":f3 [label="Constant"]
 "Altitude PID"[shape=record
 label="<f0>Altitude PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=9804.000] |<f5> [Ki=817.000] |<f6> [Kd=7353.000]"]
 "VRPN Alt" -> "Altitude PID":f1 [label="Constant"]
 "Alt Setpoint" -> "Altitude PID":f2 [label="Constant"]
-"Ts_position" -> "Altitude PID":f3 [label="Constant"]
+"Ts_VRPN" -> "Altitude PID":f3 [label="Constant"]
 "X Setpoint"[shape=record
 label="<f0>X Setpoint  |<f1> [Constant=0.000]"]
 "Y Setpoint"[shape=record
@@ -104,8 +104,8 @@ label="<f0>Signal Mixer  |<f1> --\>Throttle |<f2> --\>Pitch |<f3> --\>Roll |<f4>
 "P PWM Clamp" -> "Signal Mixer":f2 [label="Bounded"]
 "R PWM Clamp" -> "Signal Mixer":f3 [label="Bounded"]
 "Y PWM Clamp" -> "Signal Mixer":f4 [label="Bounded"]
-"Ts_angle"[shape=record
-label="<f0>Ts_angle  |<f1> [Constant=0.005]"]
-"Ts_position"[shape=record
-label="<f0>Ts_position  |<f1> [Constant=0.100]"]
+"Ts_IMU"[shape=record
+label="<f0>Ts_IMU  |<f1> [Constant=0.000]"]
+"Ts_VRPN"[shape=record
+label="<f0>Ts_VRPN  |<f1> [Constant=0.000]"]
 }
\ No newline at end of file
diff --git a/quad/sw/modular_quad_pid/gen_diagram/network.png b/quad/sw/modular_quad_pid/gen_diagram/network.png
index 86e48ace55c7bf76f2b50525988701848631e327..b0ce1b97f07fdecf57010391cfc0904fbfc1f814 100644
Binary files a/quad/sw/modular_quad_pid/gen_diagram/network.png and b/quad/sw/modular_quad_pid/gen_diagram/network.png differ
diff --git a/quad/sw/modular_quad_pid/src/control_algorithm.c b/quad/sw/modular_quad_pid/src/control_algorithm.c
index bc2f193ae88ba91f08ea9379282c9c41688e9528..0d3d6f4e4269005355da5403df80c4757689d1f6 100644
--- a/quad/sw/modular_quad_pid/src/control_algorithm.c
+++ b/quad/sw/modular_quad_pid/src/control_algorithm.c
@@ -90,29 +90,29 @@ int control_algorithm_init(parameter_t * ps)
 
     ps->mixer = graph_add_node_mixer(graph, "Signal Mixer");
 
-    ps->angle_time = graph_add_node_const(graph, "Ts_angle");
-    ps->pos_time = graph_add_node_const(graph, "Ts_position");
+    ps->angle_time = graph_add_node_const(graph, "Ts_IMU");
+    ps->pos_time = graph_add_node_const(graph, "Ts_VRPN");
 
     // Connect pitch PID chain
     graph_set_source(graph, ps->pitch_r_pid, PID_SETPOINT, ps->pitch_pid, PID_CORRECTION);
     graph_set_source(graph, ps->pitch_r_pid, PID_CUR_POINT, ps->theta_dot, CONST_VAL);
     graph_set_source(graph, ps->pitch_r_pid, PID_DT, ps->angle_time, CONST_VAL);
-    //graph_set_source(graph, ps->pitch_pid, PID_SETPOINT, ps->rc_pitch, CONST_VAL);
-    graph_set_source(graph, ps->pitch_pid, PID_CUR_POINT, ps->cur_pitch, CONST_VAL);
-    graph_set_source(graph, ps->pitch_pid, PID_DT, ps->angle_time, CONST_VAL);
+    //graph_set_source(graph, ps->pitch_pid, PID_CUR_POINT, ps->cur_pitch, CONST_VAL);
+    graph_set_source(graph, ps->pitch_pid, PID_CUR_POINT, ps->vrpn_pitch, CONST_VAL);
+    graph_set_source(graph, ps->pitch_pid, PID_DT, ps->pos_time, CONST_VAL);
 
      // Connect roll PID chain
     graph_set_source(graph, ps->roll_r_pid, PID_SETPOINT, ps->roll_pid, PID_CORRECTION);
     graph_set_source(graph, ps->roll_r_pid, PID_CUR_POINT, ps->phi_dot, CONST_VAL);
     graph_set_source(graph, ps->roll_r_pid, PID_DT, ps->angle_time, CONST_VAL);
-    //graph_set_source(graph, ps->roll_pid, PID_SETPOINT, ps->rc_roll, CONST_VAL);
-    graph_set_source(graph, ps->roll_pid, PID_CUR_POINT, ps->cur_roll, CONST_VAL);
-    graph_set_source(graph, ps->roll_pid, PID_DT, ps->angle_time, CONST_VAL);
+    //graph_set_source(graph, ps->roll_pid, PID_CUR_POINT, ps->cur_roll, CONST_VAL);
+    graph_set_source(graph, ps->roll_pid, PID_CUR_POINT, ps->vrpn_roll, CONST_VAL);
+    graph_set_source(graph, ps->roll_pid, PID_DT, ps->pos_time, CONST_VAL);
 
     // Connect yaw PID chain
     graph_set_source(graph, ps->yaw_r_pid, PID_SETPOINT, ps->rc_yaw, PID_CORRECTION);
     graph_set_source(graph, ps->yaw_r_pid, PID_CUR_POINT, ps->psi_dot, CONST_VAL);
-    graph_set_source(graph, ps->yaw_r_pid, PID_DT, ps->angle_time, CONST_VAL);
+    graph_set_source(graph, ps->yaw_r_pid, PID_DT, ps->pos_time, CONST_VAL);
 
 
     // X autonomous
@@ -275,9 +275,13 @@ int control_algorithm_init(parameter_t * ps)
 	// Sensor values
     graph_set_param_val(graph, ps->cur_pitch, CONST_SET, sensor_struct->pitch_angle_filtered);
     graph_set_param_val(graph, ps->cur_roll, CONST_SET, sensor_struct->roll_angle_filtered);
-    graph_set_param_val(graph, ps->theta_dot, CONST_SET, sensor_struct->theta_dot);
-    graph_set_param_val(graph, ps->phi_dot, CONST_SET, sensor_struct->phi_dot);
-    graph_set_param_val(graph, ps->psi_dot, CONST_SET, sensor_struct->psi_dot);
+    //graph_set_param_val(graph, ps->theta_dot, CONST_SET, sensor_struct->theta_dot);
+    //graph_set_param_val(graph, ps->phi_dot, CONST_SET, sensor_struct->phi_dot);
+    //graph_set_param_val(graph, ps->psi_dot, CONST_SET, sensor_struct->psi_dot);
+    graph_set_param_val(graph, ps->theta_dot, CONST_SET, sensor_struct->gyr_y);
+    graph_set_param_val(graph, ps->phi_dot, CONST_SET, sensor_struct->gyr_x);
+    graph_set_param_val(graph, ps->psi_dot, CONST_SET, sensor_struct->gyr_z);
+
     // RC values
     graph_set_param_val(graph, ps->rc_pitch, CONST_SET, user_input_struct->pitch_angle_manual_setpoint);
     graph_set_param_val(graph, ps->rc_roll, CONST_SET, user_input_struct->roll_angle_manual_setpoint);
diff --git a/quad/sw/modular_quad_pid/src/sensor_processing.c b/quad/sw/modular_quad_pid/src/sensor_processing.c
index 1a69d261624f692b55bf4dd6d295f40afe9ee425..d7bd7b707ac23b2b87e8c5aab8029840dce80b31 100644
--- a/quad/sw/modular_quad_pid/src/sensor_processing.c
+++ b/quad/sw/modular_quad_pid/src/sensor_processing.c
@@ -79,6 +79,11 @@ int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_se
 	sensor_struct->psi_dot = (raw_sensor_struct->gam.gyro_yVel_q*sin_phi*sec_theta)
 			+ (raw_sensor_struct->gam.gyro_zVel_r*cos_phi*sec_theta);
 
+	// Copy in raw gyroscope values
+	sensor_struct->gyr_x = raw_sensor_struct->gam.gyro_xVel_p;
+	sensor_struct->gyr_y = raw_sensor_struct->gam.gyro_yVel_q;
+	sensor_struct->gyr_z = raw_sensor_struct->gam.gyro_zVel_r;
+
 	// Complementary Filter Calculations
 	sensor_struct->pitch_angle_filtered = 0.98 * (sensor_struct->pitch_angle_filtered + sensor_struct->theta_dot * get_last_loop_time())
 			+ 0.02 * raw_sensor_struct->gam.accel_pitch;
diff --git a/quad/sw/modular_quad_pid/src/type_def.h b/quad/sw/modular_quad_pid/src/type_def.h
index 40c0db97f613724b0fa92fef886bc359b0c9bd7f..e67da1705fa520188e93457fa9cd70c219a45d9f 100644
--- a/quad/sw/modular_quad_pid/src/type_def.h
+++ b/quad/sw/modular_quad_pid/src/type_def.h
@@ -254,13 +254,13 @@ typedef struct sensor {
 	int acc_z_t;	// time of accelerometer z data
 
 
-	int gyr_x;		// gyroscope x data
+	float gyr_x;		// gyroscope x data
 	int gyr_x_t;	// time of gyroscope x data
 
-	int gyr_y;		// gyroscope y data
+	float gyr_y;		// gyroscope y data
 	int gyr_y_t;	// time of gyroscope y data
 
-	int gyr_z;		// gyroscope z data
+	float gyr_z;		// gyroscope z data
 	int gyr_z_t;	// time of gyroscope z data
 
 	int ldr_z;		//lidar z data (altitude)