diff --git a/quad/src/gen_diagram/network.dot b/quad/src/gen_diagram/network.dot
index c7ebd3547075c6e2210eef8a3855bb61659a84c3..132e3de85641396dd4810289a15803411824e3db 100644
--- a/quad/src/gen_diagram/network.dot
+++ b/quad/src/gen_diagram/network.dot
@@ -3,12 +3,12 @@ rankdir="LR"
"Roll PID"[shape=record
label="<f0>Roll PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=35.000] |<f5> [Ki=0.000] |<f6> [Kd=1.000] |<f7> [alpha=0.880]"]
"Roll" -> "Roll PID":f1 [label="Constant"]
-"RC Roll" -> "Roll PID":f2 [label="Constant"]
+"Yaw Correction" -> "Roll PID":f2 [label="Rotated Y"]
"Ts_IMU" -> "Roll PID":f3 [label="Constant"]
"Pitch PID"[shape=record
label="<f0>Pitch PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=35.000] |<f5> [Ki=0.000] |<f6> [Kd=1.000] |<f7> [alpha=0.880]"]
"Pitch trim add" -> "Pitch PID":f1 [label="Sum"]
-"RC Pitch" -> "Pitch PID":f2 [label="Constant"]
+"Yaw Correction" -> "Pitch PID":f2 [label="Rotated X"]
"Ts_IMU" -> "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] |<f7> [alpha=0.000]"]
@@ -28,7 +28,7 @@ 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=0.297] |<f5> [Ki=0.000] |<f6> [Kd=0.000] |<f7> [alpha=0.000]"]
"Gyro Z" -> "Yaw Rate PID":f1 [label="Constant"]
-"RC Yaw" -> "Yaw Rate PID":f2 [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.550] |<f5> [Ki=0.007] |<f6> [Kd=0.000] |<f7> [alpha=0.000]"]
@@ -135,15 +135,9 @@ label="<f0>Yaw Correction |<f1> --\>Current Yaw |<f2> --\>X Position |<f3> --\>
"Y vel Clamp" -> "Yaw Correction":f3 [label="Bounded"]
"OF Offset Angle"[shape=record
label="<f0>OF Offset Angle |<f1> --\>Current Yaw |<f2> --\>X Position |<f3> --\>Y Position"]
-"OF Offset Add" -> "OF Offset Angle":f1 [label="Sum"]
+"Integrated gyro z" -> "OF Offset Angle":f1 [label="Integrated"]
"Flow Vel X" -> "OF Offset Angle":f2 [label="Constant"]
"Flow Vel Y" -> "OF Offset Angle":f3 [label="Constant"]
-"OF Offset Rot"[shape=record
-label="<f0>OF Offset Rot |<f1> [Constant=0.622]"]
-"OF Offset Add"[shape=record
-label="<f0>OF Offset Add |<f1> --\>Summand 1 |<f2> --\>Summand 2"]
-"OF Offset Rot" -> "OF Offset Add":f1 [label="Constant"]
-"Integrated gyro z" -> "OF Offset Add":f2 [label="Integrated"]
"OF Integrate X"[shape=record
label="<f0>OF Integrate X |<f1> --\>Integrator In |<f2> --\>Integrator dt"]
"OF Offset Angle" -> "OF Integrate X":f1 [label="Rotated X"]
@@ -169,7 +163,7 @@ label="<f0>Integrated gyro z |<f1> --\>Integrator In |<f2> --\>Integrator dt"]
"Gyro Z" -> "Integrated gyro z":f1 [label="Constant"]
"Signal Mixer"[shape=record
label="<f0>Signal Mixer |<f1> --\>Throttle |<f2> --\>Pitch |<f3> --\>Roll |<f4> --\>Yaw"]
-"RC Throttle" -> "Signal Mixer":f1 [label="Constant"]
+"T trim add" -> "Signal Mixer":f1 [label="Sum"]
"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/src/gen_diagram/network.png b/quad/src/gen_diagram/network.png
index 8a85dddca8320d59cc742c69825d449673b3577f..d0c2ae5e41d0e3108c5c5480eb0748e18ffd2de0 100644
Binary files a/quad/src/gen_diagram/network.png and b/quad/src/gen_diagram/network.png differ
diff --git a/quad/src/quad_app/control_algorithm.c b/quad/src/quad_app/control_algorithm.c
index 6f1f86a2f4e97fc8434e6ead0058ecb2d7d183ba..05e4b4b74e09383f94d6e7a6683179aa00f856db 100644
--- a/quad/src/quad_app/control_algorithm.c
+++ b/quad/src/quad_app/control_algorithm.c
@@ -22,7 +22,6 @@
#define ANGLE_CLAMP (0.1745)
#define PX4FLOW_QUAL_MIN (100)
-#define OF_OFFSET_ANGLE (0.62204) // 35.64 degrees
#define ROLL_PITCH_MAX_ANGLE 0.35 // 20 degrees
#define PWM_DIFF_BOUNDS 20000
@@ -119,8 +118,6 @@ int control_algorithm_init(parameter_t * ps)
// Optical Flow
ps->of_angle_corr = graph_add_defined_block(graph, BLOCK_YAW_ROT, "OF Offset Angle");
- ps->of_angle_offset = graph_add_defined_block(graph, BLOCK_CONSTANT, "OF Offset Rot");
- ps->of_angle_add = graph_add_defined_block(graph, BLOCK_ADD, "OF Offset Add");
ps->of_integ_x = graph_add_defined_block(graph, BLOCK_INTEGRATOR, "OF Integrate X");
ps->of_integ_y = graph_add_defined_block(graph, BLOCK_INTEGRATOR, "OF Integrate Y");
ps->of_trim_x = graph_add_defined_block(graph, BLOCK_CONSTANT, "OF Trim X");
@@ -275,13 +272,11 @@ int control_algorithm_init(parameter_t * ps)
graph_set_source(graph, ps->mixer, MIXER_YAW, ps->clamp_d_pwmY, BOUNDS_OUT);
// Connect optical flow
- graph_set_source(graph, ps->of_angle_add, ADD_SUMMAND1, ps->of_angle_offset, CONST_VAL);
#ifdef USE_FAKE_YAW
- graph_set_source(graph, ps->of_angle_add, ADD_SUMMAND2, ps->gyro_yaw, CONST_VAL);
+ graph_set_source(graph, ps->of_angle_corr, ROT_YAW, ps->gyro_yaw, ADD_SUM);
#else
- graph_set_source(graph, ps->of_angle_add, ADD_SUMMAND2, ps->cur_yaw, CONST_VAL);
+ graph_set_source(graph, ps->of_angle_corr, ROT_YAW, ps->cur_yaw, ADD_SUM);
#endif
- graph_set_source(graph, ps->of_angle_corr, ROT_YAW, ps->of_angle_add, ADD_SUM);
graph_set_source(graph, ps->of_angle_corr, ROT_CUR_X, ps->flow_vel_x, CONST_VAL);
graph_set_source(graph, ps->of_angle_corr, ROT_CUR_Y, ps->flow_vel_y, CONST_VAL);
// Integration
@@ -371,7 +366,6 @@ int control_algorithm_init(parameter_t * ps)
// Set trims
graph_set_param_val(graph, ps->pitch_trim, CONST_SET, PITCH_TRIM);
- graph_set_param_val(graph, ps->of_angle_offset, CONST_SET, OF_OFFSET_ANGLE);
// Initial value for sampling periods
graph_set_param_val(graph, ps->pos_time, CONST_SET, 0.04);
diff --git a/quad/src/quad_app/type_def.h b/quad/src/quad_app/type_def.h
index 9c3e651305954b6a971485c1f24ad76f1acb9ff6..c424869c4574c8445259bc3a1713178cae0ba898 100644
--- a/quad/src/quad_app/type_def.h
+++ b/quad/src/quad_app/type_def.h
@@ -404,8 +404,6 @@ typedef struct parameter_t {
// Sensor processing
int yaw_correction;
int of_angle_corr; // Corrects for the optical flow mounting angle
- int of_angle_offset; // Offset for optical flow angle
- int of_angle_add; // Adds optical flow static offset to current yaw
int of_integ_x; // Integrates the optical flow data
int of_integ_y;
int of_trim_x; // Trim value for optical flow integrated value