Removing optical flow constant offset, since it has been remounted.

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"]

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);

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