Enabled yaw controller for autonomous flight.

quad/sw/modular_quad_pid/gen_diagram/network.dot

@@ -3,18 +3,18 @@ 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]"]
 "VRPN Roll" -> "Roll PID":f1 [label="Constant"]
-"RC Roll" -> "Roll PID":f2 [label="Constant"]
+"Y pos PID" -> "Roll PID":f2 [label="Correction"]
 "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]"]
 "VRPN Pitch" -> "Pitch PID":f1 [label="Constant"]
-"RC Pitch" -> "Pitch PID":f2 [label="Constant"]
+"X pos PID" -> "Pitch PID":f2 [label="Correction"]
 "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_IMU" -> "Yaw PID":f3 [label="Constant"]
+"Ts_VRPN" -> "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"]
@@ -28,8 +28,8 @@ 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=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_VRPN" -> "Yaw Rate PID":f3 [label="Constant"]
+"Yaw PID" -> "Yaw Rate PID":f2 [label="Correction"]
+"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.015] |<f5> [Ki=-0.005] |<f6> [Kd=-0.030]"]
 "VRPN X" -> "X pos PID":f1 [label="Constant"]
@@ -100,7 +100,6 @@ label="<f0>RC Yaw  |<f1> [Constant=0.000]"]
 label="<f0>RC Throttle  |<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"]
 "P PWM Clamp" -> "Signal Mixer":f2 [label="Bounded"]
 "R PWM Clamp" -> "Signal Mixer":f3 [label="Bounded"]
 "Y PWM Clamp" -> "Signal Mixer":f4 [label="Bounded"]

quad/sw/modular_quad_pid/src/control_algorithm.c

@@ -26,7 +26,7 @@ void connect_autonomous(parameter_t* ps) {
 	graph_set_source(graph, ps->pitch_pid, PID_SETPOINT, ps->x_pos_pid, PID_CORRECTION);
 	graph_set_source(graph, ps->roll_pid, PID_SETPOINT, ps->y_pos_pid, PID_CORRECTION);
 	//graph_set_source(graph, ps->mixer, MIXER_THROTTLE, ps->throttle_trim_add, ADD_SUM);
-	//graph_set_source(graph, ps->yaw_r_pid, PID_SETPOINT, ps->yaw_pid, PID_CORRECTION);
+	graph_set_source(graph, ps->yaw_r_pid, PID_SETPOINT, ps->yaw_pid, PID_CORRECTION);
 }
 
 void connect_manual(parameter_t* ps) {
@@ -112,7 +112,7 @@ int control_algorithm_init(parameter_t * ps)
     // 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->pos_time, CONST_VAL);
+    graph_set_source(graph, ps->yaw_r_pid, PID_DT, ps->angle_time, CONST_VAL);
 
 
     // X autonomous
@@ -131,7 +131,7 @@ int control_algorithm_init(parameter_t * ps)
     graph_set_source(graph, ps->throttle_trim_add, ADD_SUMMAND1, ps->alt_pid, PID_CORRECTION);
     graph_set_source(graph, ps->throttle_trim_add, ADD_SUMMAND2, ps->throttle_trim, CONST_VAL);
     // Yaw autonomous
-    graph_set_source(graph, ps->yaw_pid, PID_DT, ps->angle_time, CONST_VAL);
+    graph_set_source(graph, ps->yaw_pid, PID_DT, ps->pos_time, CONST_VAL);
     graph_set_source(graph, ps->yaw_pid, PID_SETPOINT, ps->yaw_set, CONST_VAL);
     graph_set_source(graph, ps->yaw_pid, PID_CUR_POINT, ps->cur_yaw, CONST_VAL);