diff --git a/quad/sw/modular_quad_pid/gen_diagram/gen_diagram b/quad/sw/modular_quad_pid/gen_diagram/gen_diagram
index 82a727a63fa1c424090b886e02feadd41d867e9b..8413df5adf030804107bf812502da5249bf17b24 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 14f0e486829eeaf7d39a17e9afadcf5323e533ad..6c7ff17339d512cd4468ece1ac8c74a7e7b86183 100644
--- a/quad/sw/modular_quad_pid/gen_diagram/network.dot
+++ b/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"]
diff --git a/quad/sw/modular_quad_pid/gen_diagram/network.png b/quad/sw/modular_quad_pid/gen_diagram/network.png
index b0ce1b97f07fdecf57010391cfc0904fbfc1f814..6734445acec05b986f97fb83855e62a39ee27463 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 0d3d6f4e4269005355da5403df80c4757689d1f6..7a92f9f6bf6b7883d37f12d4f9cbc0dc207041fc 100644
--- a/quad/sw/modular_quad_pid/src/control_algorithm.c
+++ b/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);