Added trimmer blocks for integrated optical flow X and Y position.

quad/src/gen_diagram/network.dot

@@ -145,11 +145,11 @@ label="<f0>Yaw Correction  |<f1> --\>Current Yaw |<f2> --\>X Position |<f3> --\>
 "Yaw" -> "Yaw Correction":f1 [label="Constant"]
 "X Vel PID" -> "Yaw Correction":f2 [label="Correction"]
 "Y Vel PID" -> "Yaw Correction":f3 [label="Correction"]
-"OF Offset"[shape=record
-label="<f0>OF Offset  |<f1> --\>Current Yaw |<f2> --\>X Position |<f3> --\>Y Position"]
-"OF Offset Add" -> "OF Offset":f1 [label="Sum"]
-"Flow Vel X" -> "OF Offset":f2 [label="Constant"]
-"Flow Vel Y" -> "OF Offset":f3 [label="Constant"]
+"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"]
+"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
@@ -158,12 +158,24 @@ label="<f0>OF Offset Add  |<f1> --\>Summand 1 |<f2> --\>Summand 2"]
 "Yaw" -> "OF Offset Add":f2 [label="Constant"]
 "OF Integrate X"[shape=record
 label="<f0>OF Integrate X  |<f1> --\>Integrator In |<f2> --\>Integrator dt"]
-"OF Offset" -> "OF Integrate X":f1 [label="Rotated X"]
+"OF Offset Angle" -> "OF Integrate X":f1 [label="Rotated X"]
 "Ts_IMU" -> "OF Integrate X":f2 [label="Constant"]
 "OF Integrate Y"[shape=record
 label="<f0>OF Integrate Y  |<f1> --\>Integrator In |<f2> --\>Integrator dt"]
-"OF Offset" -> "OF Integrate Y":f1 [label="Rotated Y"]
+"OF Offset Angle" -> "OF Integrate Y":f1 [label="Rotated Y"]
 "Ts_IMU" -> "OF Integrate Y":f2 [label="Constant"]
+"OF Trim X"[shape=record
+label="<f0>OF Trim X  |<f1> [Constant=0.000]"]
+"OF Trim Y"[shape=record
+label="<f0>OF Trim Y  |<f1> [Constant=0.000]"]
+"OF X Trim Add"[shape=record
+label="<f0>OF X Trim Add  |<f1> --\>Summand 1 |<f2> --\>Summand 2"]
+"OF Integrate X" -> "OF X Trim Add":f1 [label="Integrated"]
+"OF Trim X" -> "OF X Trim Add":f2 [label="Constant"]
+"OF Y Trim Add"[shape=record
+label="<f0>OF Y Trim Add  |<f1> --\>Summand 1 |<f2> --\>Summand 2"]
+"OF Integrate Y" -> "OF Y Trim Add":f1 [label="Integrated"]
+"OF Trim Y" -> "OF Y Trim Add":f2 [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"]

quad/src/quad_app/control_algorithm.c

@@ -108,11 +108,15 @@ int control_algorithm_init(parameter_t * ps)
     ps->yaw_correction = graph_add_defined_block(graph, BLOCK_YAW_ROT, "Yaw Correction");
 
     // Optical Flow
-    ps->of_angle_corr = graph_add_defined_block(graph, BLOCK_YAW_ROT, "OF Offset");
+    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");
+    ps->of_trim_y = graph_add_defined_block(graph, BLOCK_CONSTANT, "OF Trim Y");
+    ps->of_trimmed_x = graph_add_defined_block(graph, BLOCK_ADD, "OF X Trim Add");
+    ps->of_trimmed_y = graph_add_defined_block(graph, BLOCK_ADD, "OF Y Trim Add");
 
     ps->mixer = graph_add_defined_block(graph, BLOCK_MIXER, "Signal Mixer");
 
@@ -225,6 +229,11 @@ int control_algorithm_init(parameter_t * ps)
     graph_set_source(graph, ps->of_integ_x, INTEGRATE_DT, ps->angle_time, CONST_VAL);
     graph_set_source(graph, ps->of_integ_y, INTEGRATE_IN, ps->of_angle_corr, ROT_OUT_Y);
     graph_set_source(graph, ps->of_integ_y, INTEGRATE_DT, ps->angle_time, CONST_VAL);
+        // Trim
+    graph_set_source(graph, ps->of_trimmed_x, ADD_SUMMAND1, ps->of_integ_x, INTEGRATED);
+    graph_set_source(graph, ps->of_trimmed_x, ADD_SUMMAND2, ps->of_trim_x, CONST_VAL);
+    graph_set_source(graph, ps->of_trimmed_y, ADD_SUMMAND1, ps->of_integ_y, INTEGRATED);
+    graph_set_source(graph, ps->of_trimmed_y, ADD_SUMMAND2, ps->of_trim_y, CONST_VAL);
 
 
     // Set pitch PID constants

quad/src/quad_app/type_def.h

@@ -390,6 +390,10 @@ typedef struct parameter_t {
 	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
+	int of_trim_y;
+	int of_trimmed_x; // Trimmed optical flow integrated value (of_integ_x + of_trim_x)
+	int of_trimmed_y;
 } parameter_t;
 
 /**