Set VRPN as source for roll/pitch.

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

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

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;