diff --git a/quad/sw/modular_quad_pid/gen_diagram/gen_diagram b/quad/sw/modular_quad_pid/gen_diagram/gen_diagram
index 8174dab245654da3b79d9ab54c835b93766a8b02..2984a72b205bab88196b1dd83121f6f73f25be37 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 00e5b058e18e15345720278726ff5faae6351bee..d92f0bc8bd3aa7e9b26e81e90ff27032b5416ae7 100644
--- a/quad/sw/modular_quad_pid/gen_diagram/network.dot
+++ b/quad/sw/modular_quad_pid/gen_diagram/network.dot
@@ -2,13 +2,13 @@ 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"]
-"Y pos PID" -> "Roll PID":f2 [label="Correction"]
+"VRPN Roll" -> "Roll PID":f1 [label="Constant"]
+"RC Roll" -> "Roll PID":f2 [label="Constant"]
 "Ts_angle" -> "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"]
-"X pos PID" -> "Pitch PID":f2 [label="Correction"]
+"VRPN Pitch" -> "Pitch PID":f1 [label="Constant"]
+"RC Pitch" -> "Pitch PID":f2 [label="Constant"]
 "Ts_angle" -> "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]"]
@@ -86,6 +86,10 @@ label="<f0>VRPN X  |<f1> [Constant=0.000]"]
 label="<f0>VRPN Y  |<f1> [Constant=0.000]"]
 "VRPN Alt"[shape=record
 label="<f0>VRPN Alt  |<f1> [Constant=0.000]"]
+"VRPN Pitch"[shape=record
+label="<f0>VRPN Pitch  |<f1> [Constant=0.000]"]
+"VRPN Roll"[shape=record
+label="<f0>VRPN Roll  |<f1> [Constant=0.000]"]
 "RC Pitch"[shape=record
 label="<f0>RC Pitch  |<f1> [Constant=0.000]"]
 "RC Roll"[shape=record
diff --git a/quad/sw/modular_quad_pid/gen_diagram/network.png b/quad/sw/modular_quad_pid/gen_diagram/network.png
index f1203a6c4dc72f46b67c1f76e06ae6a4c93eb2d4..7e35c5402e1e36381e4cbd07418c1f6567784291 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 632cde85131d83bd2a5c811c8ce3124b02abe351..b2498e6c29030f62db112dd6c815a508aac3b463 100644
--- a/quad/sw/modular_quad_pid/src/control_algorithm.c
+++ b/quad/sw/modular_quad_pid/src/control_algorithm.c
@@ -76,6 +76,8 @@ int control_algorithm_init(parameter_t * ps)
     ps->vrpn_x = graph_add_node_const(graph, "VRPN X");
     ps->vrpn_y = graph_add_node_const(graph, "VRPN Y");
     ps->vrpn_alt = graph_add_node_const(graph, "VRPN Alt");
+    ps->vrpn_pitch = graph_add_node_const(graph, "VRPN Pitch");
+    ps->vrpn_roll = graph_add_node_const(graph, "VRPN Roll");
 
     // Create blocks for RC controller
     ps->rc_pitch = graph_add_node_const(graph, "RC Pitch");
@@ -93,7 +95,7 @@ int control_algorithm_init(parameter_t * ps)
     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_CUR_POINT, ps->vrpn_pitch, CONST_VAL);
     graph_set_source(graph, ps->pitch_pid, PID_DT, ps->angle_time, CONST_VAL);
 
      // Connect roll PID chain
@@ -101,7 +103,7 @@ int control_algorithm_init(parameter_t * ps)
     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_CUR_POINT, ps->vrpn_roll, CONST_VAL);
     graph_set_source(graph, ps->roll_pid, PID_DT, ps->angle_time, CONST_VAL);
 
     // Connect yaw PID chain
@@ -259,6 +261,8 @@ int control_algorithm_init(parameter_t * ps)
 	    graph_set_param_val(graph, ps->vrpn_x, CONST_SET, sensor_struct->currentQuadPosition.x_pos);
 	    graph_set_param_val(graph, ps->vrpn_y, CONST_SET, sensor_struct->currentQuadPosition.y_pos);
 	    graph_set_param_val(graph, ps->vrpn_alt, CONST_SET, sensor_struct->currentQuadPosition.alt_pos);
+	    graph_set_param_val(graph, ps->vrpn_pitch, CONST_SET, sensor_struct->currentQuadPosition.pitch);
+	    graph_set_param_val(graph, ps->vrpn_roll, CONST_SET, sensor_struct->currentQuadPosition.roll);
 	    graph_set_param_val(graph, ps->cur_yaw, CONST_SET, sensor_struct->currentQuadPosition.yaw);
 	}
 	// Sensor values
diff --git a/quad/sw/modular_quad_pid/src/type_def.h b/quad/sw/modular_quad_pid/src/type_def.h
index cf893938b9cf34327f543ab2b607bf352c6e89a5..10edff1f2fc656dd25a047f0e3032216e2eda67e 100644
--- a/quad/sw/modular_quad_pid/src/type_def.h
+++ b/quad/sw/modular_quad_pid/src/type_def.h
@@ -311,6 +311,7 @@ typedef struct parameter_t {
 	int vrpn_x;
 	int vrpn_y;
 	int vrpn_alt;
+	int vrpn_pitch, vrpn_roll;
 	// RC blocks
 	int rc_pitch;
 	int rc_roll;