Merge remote-tracking branch 'origin/quad-yaw-fix2'

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"]
-"Y Vel PID" -> "Roll PID":f2 [label="Correction"]
+"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"]
-"X Vel PID" -> "Pitch PID":f2 [label="Correction"]
+"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]"]
@@ -67,6 +67,12 @@ label="<f0>Roll  |<f1> [Constant=0.000]"]
 label="<f0>Yaw  |<f1> [Constant=0.000]"]
 "Lidar"[shape=record
 label="<f0>Lidar  |<f1> [Constant=0.000]"]
+"Flow Vel X"[shape=record
+label="<f0>Flow Vel X  |<f1> [Constant=0.000]"]
+"Flow Vel y"[shape=record
+label="<f0>Flow Vel y  |<f1> [Constant=0.000]"]
+"Flow Quality"[shape=record
+label="<f0>Flow Quality  |<f1> [Constant=0.000]"]
 "Pitch trim"[shape=record
 label="<f0>Pitch trim  |<f1> [Constant=0.045]"]
 "Pitch trim add"[shape=record
@@ -132,6 +138,11 @@ label="<f0>X Vel Clamp  |<f1> --\>Bounds in |<f2> [Min=-2.000] |<f3> [Max=2.000]
 "Y vel Clamp"[shape=record
 label="<f0>Y vel Clamp  |<f1> --\>Bounds in |<f2> [Min=-2.000] |<f3> [Max=2.000]"]
 "Y pos PID" -> "Y vel Clamp":f1 [label="Correction"]
+"Yaw Correction"[shape=record
+label="<f0>Yaw Correction  |<f1> --\>Current Yaw |<f2> --\>X Position |<f3> --\>Y Position"]
+"Yaw" -> "Yaw Correction":f1 [label="Constant"]
+"X Vel PID" -> "Yaw Correction":f2 [label="Correction"]
+"Y Vel PID" -> "Yaw Correction":f3 [label="Correction"]
 "Signal Mixer"[shape=record
 label="<f0>Signal Mixer  |<f1> --\>Throttle |<f2> --\>Pitch |<f3> --\>Roll |<f4> --\>Yaw"]
 "T trim add" -> "Signal Mixer":f1 [label="Sum"]

quad/src/graph_blocks/graph_blocks.c

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

quad/src/graph_blocks/graph_blocks.h

@@ -10,6 +10,7 @@
 #include "node_bounds.h"
 #include "node_mixer.h"
 #include "node_pid.h"
+#include "node_yaw_rot.h"
 
 /*
  * ---------- How-To ------------
@@ -35,6 +36,7 @@ enum BlockTypes {
     BLOCK_BOUNDS,        // 05
     BLOCK_MIXER,         // 06
     BLOCK_PID,           // 07
+    BLOCK_YAW_ROT,       // 08
     //                        <-- Insert new block type here
     MAX_BLOCK_TYPES
 };

quad/src/graph_blocks/node_accumulator.c

@@ -32,10 +32,3 @@ const struct graph_node_type node_accum_type = {
         .type_id = BLOCK_ACCUMULATE
 };
 
-int graph_add_node_accum(struct computation_graph *graph, const char* name) {
-    struct accum_state* node_state = malloc(sizeof(struct accum_state));
-    if (sizeof(struct accum_state) && !node_state) {
-        return -1; // malloc failed
-    }
-    return graph_add_node(graph, name, &node_accum_type, node_state);
-}

quad/src/graph_blocks/node_accumulator.h

@@ -3,8 +3,6 @@
 #include "computation_graph.h"
 #include "graph_blocks.h"
 
-int graph_add_node_accum(struct computation_graph *graph, const char* name);
-
 extern const struct graph_node_type node_accum_type;
 
 enum graph_node_accum_inputs {

quad/src/graph_blocks/node_add.c

@@ -22,6 +22,3 @@ const struct graph_node_type node_add_type = {
         .type_id = BLOCK_ADD
 };
 
-int graph_add_node_add(struct computation_graph *graph, const char* name) {
-    return graph_add_node(graph, name, &node_add_type, NULL);
-}

quad/src/graph_blocks/node_add.h

@@ -3,8 +3,6 @@
 #include "computation_graph.h"
 #include "graph_blocks.h"
 
-int graph_add_node_add(struct computation_graph *graph, const char* name);
-
 extern const struct graph_node_type node_add_type;
 
 enum graph_node_add_inputs {

quad/src/graph_blocks/node_bounds.c

@@ -29,7 +29,3 @@ const struct graph_node_type node_bounds_type = {
         .state_size = 0,
         .type_id = BLOCK_BOUNDS
 };
-
-int graph_add_node_bounds(struct computation_graph *graph, const char* name) {
-    return graph_add_node(graph, name, &node_bounds_type, NULL);
-}

quad/src/graph_blocks/node_bounds.h

@@ -3,7 +3,6 @@
 #include "computation_graph.h"
 #include "graph_blocks.h"
 
-int graph_add_node_bounds(struct computation_graph *graph, const char* name);
 
 extern const struct graph_node_type node_bounds_type;
 

quad/src/graph_blocks/node_constant.c

@@ -21,7 +21,3 @@ const struct graph_node_type node_const_type = {
         .state_size = 0,
         .type_id = BLOCK_CONSTANT
 };
-
-int graph_add_node_const(struct computation_graph *graph, const char* name) {
-    return graph_add_node(graph, name, &node_const_type, NULL);
-}

quad/src/graph_blocks/node_constant.h

@@ -3,8 +3,6 @@
 #include "computation_graph.h"
 #include "graph_blocks.h"
 
-int graph_add_node_const(struct computation_graph *graph, const char* name);
-
 extern const struct graph_node_type node_const_type;
 
 enum graph_node_const_params {

quad/src/graph_blocks/node_gain.c

@@ -21,7 +21,3 @@ const struct graph_node_type node_gain_type = {
         .state_size = 0,
         .type_id = BLOCK_GAIN
 };
-
-int graph_add_node_gain(struct computation_graph *graph, const char* name) {
-    return graph_add_node(graph, name, &node_gain_type, NULL);
-}

quad/src/graph_blocks/node_gain.h

@@ -3,8 +3,6 @@
 #include "computation_graph.h"
 #include "graph_blocks.h"
 
-int graph_add_node_gain(struct computation_graph *graph, const char* name);
-
 extern const struct graph_node_type node_gain_type;
 
 enum graph_node_pow_inputs {

quad/src/graph_blocks/node_mixer.c

@@ -38,7 +38,3 @@ const struct graph_node_type node_mixer_type = {
 		.state_size = 0,
 		.type_id = BLOCK_MIXER
 };
-
-int graph_add_node_mixer(struct computation_graph *graph, const char* name) {
-    return graph_add_node(graph, name, &node_mixer_type, NULL);
-}

quad/src/graph_blocks/node_mixer.h

@@ -3,8 +3,6 @@
 #include "computation_graph.h"
 #include "graph_blocks.h"
 
-int graph_add_node_mixer(struct computation_graph *graph, const char* name);
-
 extern const struct graph_node_type node_mixer_type;
 
 enum graph_node_mixer_inputs {

quad/src/graph_blocks/node_mult.c

@@ -21,7 +21,3 @@ const struct graph_node_type node_mult_type = {
         .state_size = 0,
         .type_id = BLOCK_MULT
 };
-
-int graph_add_node_mult(struct computation_graph *graph, const char* name) {
-    return graph_add_node(graph, name, &node_mult_type, NULL);
-}

quad/src/graph_blocks/node_mult.h

@@ -3,8 +3,6 @@
 #include "computation_graph.h"
 #include "graph_blocks.h"
 
-int graph_add_node_mult(struct computation_graph *graph, const char* name);
-
 extern const struct graph_node_type node_mult_type;
 
 enum graph_node_mult_inputs {

quad/src/graph_blocks/node_pid.c

@@ -101,11 +101,3 @@ const struct graph_node_type node_pid_type = {
         .state_size = sizeof(struct pid_node_state),
         .type_id = BLOCK_PID
 };
-
-int graph_add_node_pid(struct computation_graph *graph, const char* name) {
-    struct pid_node_state* node_state = malloc(sizeof(struct pid_node_state));
-    if (sizeof(struct pid_node_state) && !node_state) {
-        return -1; // malloc failed
-    }
-    return graph_add_node(graph, name, &node_pid_type, node_state);
-}

quad/src/graph_blocks/node_pid.h

@@ -3,8 +3,6 @@
 #include "computation_graph.h"
 #include "graph_blocks.h"
 
-int graph_add_node_pid(struct computation_graph *graph, const char* name);
-
 extern const struct graph_node_type node_pid_type;
 
 enum graph_node_pid_inputs {