Created integrator block and connected optical flow output to integrator blocks.

quad/src/gen_diagram/network.dot

@@ -156,6 +156,14 @@ label="<f0>OF Offset Rot  |<f1> [Constant=-0.622]"]
 label="<f0>OF Offset Add  |<f1> --\>Summand 1 |<f2> --\>Summand 2"]
 "OF Offset Rot" -> "OF Offset Add":f1 [label="Constant"]
 "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"]
+"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"]
+"Ts_IMU" -> "OF Integrate Y":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/graph_blocks/graph_blocks.c

@@ -12,7 +12,8 @@ const struct graph_node_type* blockDefs[MAX_BLOCK_TYPES] =
     &node_bounds_type,
     &node_mixer_type,
     &node_pid_type,
-    &node_yaw_rot_type
+    &node_yaw_rot_type,
+    &node_integrator_type
 };
 
 

quad/src/graph_blocks/graph_blocks.h

@@ -11,6 +11,7 @@
 #include "node_mixer.h"
 #include "node_pid.h"
 #include "node_yaw_rot.h"
+#include "node_integrator.h"
 
 /*
  * ---------- How-To ------------
@@ -37,6 +38,7 @@ enum BlockTypes {
     BLOCK_MIXER,         // 06
     BLOCK_PID,           // 07
     BLOCK_YAW_ROT,       // 08
+    BLOCK_INTEGRATOR,    // 09
     //                        <-- Insert new block type here
     MAX_BLOCK_TYPES
 };

quad/src/graph_blocks/node_integrator.c

+#include "node_integrator.h"
+#include <stdlib.h>
+
+struct integrate_state {
+    double integrated;
+};
+
+static void integrate(void *state, const double* params, const double *inputs, double *outputs) {
+    struct integrate_state* my_state = (struct integrate_state*)state;
+    my_state->integrated += inputs[INTEGRATE_IN] * inputs[INTEGRATE_DT];
+    outputs[INTEGRATED] = my_state->integrated;
+}
+
+static void reset(void *state) {
+    ((struct integrate_state*)state)->integrated = 0;
+}
+
+
+static const char* const in_names[2] = {"Integrator In", "Integrator dt"};
+static const char* const out_names[1] = {"Integrated"};
+static const char* const param_names[0] = {};
+const struct graph_node_type node_integrator_type = {
+        .input_names = in_names,
+        .output_names = out_names,
+        .param_names = param_names,
+        .n_inputs = 2,
+        .n_outputs = 1,
+        .n_params = 0,
+        .execute = integrate,
+        .reset = reset,
+        .state_size = sizeof(struct integrate_state),
+        .type_id = BLOCK_INTEGRATOR
+};
+

quad/src/graph_blocks/node_integrator.h

+#ifndef __NODE_INTEGRATOR_H__
+#define __NODE_INTEGRATOR_H__
+#include "computation_graph.h"
+#include "graph_blocks.h"
+
+extern const struct graph_node_type node_integrator_type;
+
+enum graph_node_integrate_inputs {
+    INTEGRATE_IN,
+    INTEGRATE_DT
+};
+
+enum graph_node_integrate_outputs {
+    INTEGRATED
+};
+#endif // __NODE_INTEGRATOR_H__

quad/src/quad_app/control_algorithm.c

@@ -110,6 +110,8 @@ int control_algorithm_init(parameter_t * ps)
     ps->of_angle_corr = graph_add_defined_block(graph, BLOCK_YAW_ROT, "OF Offset");
     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->mixer = graph_add_defined_block(graph, BLOCK_MIXER, "Signal Mixer");
 
@@ -217,6 +219,11 @@ int control_algorithm_init(parameter_t * ps)
     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
+    graph_set_source(graph, ps->of_integ_x, INTEGRATE_IN, ps->of_angle_corr, ROT_OUT_X);
+    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);
 
 
     // Set pitch PID constants

quad/src/quad_app/type_def.h

@@ -388,6 +388,8 @@ typedef struct parameter_t {
 	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;
 } parameter_t;
 
 /**