From 18f9feb27ec794794078099b25c8f3adf3052b77 Mon Sep 17 00:00:00 2001
From: David Wehr <dawehr@iastate.edu>
Date: Fri, 24 Mar 2017 21:17:53 -0500
Subject: [PATCH] Put all graph block definitions into graph_blocks library and
updated project to use it.
---
quad/Makefile | 2 +
quad/src/computation_graph/Makefile | 2 +-
.../src/computation_graph/computation_graph.h | 17 ++--
.../test/test_computation_graph.c | 70 ++++++++---------
quad/src/graph_blocks/.gitignore | 1 +
quad/src/graph_blocks/Makefile | 6 ++
quad/src/graph_blocks/graph_blocks.c | 33 ++++++++
quad/src/graph_blocks/graph_blocks.h | 51 ++++++++++++
.../node_accumulator.c | 3 +-
.../node_accumulator.h | 0
.../node_add.c | 3 +-
.../node_add.h | 0
.../{quad_app => graph_blocks}/node_bounds.c | 3 +-
.../{quad_app => graph_blocks}/node_bounds.h | 0
.../node_constant.c | 3 +-
.../node_constant.h | 0
.../node_gain.c | 3 +-
.../node_gain.h | 0
.../{quad_app => graph_blocks}/node_mixer.c | 3 +-
.../{quad_app => graph_blocks}/node_mixer.h | 0
.../node_mult.c | 3 +-
.../node_mult.h | 0
.../src/{quad_app => graph_blocks}/node_pid.c | 3 +-
.../src/{quad_app => graph_blocks}/node_pid.h | 0
.../node_pow.c | 3 +-
.../node_pow.h | 0
quad/src/quad_app/Makefile | 2 +-
quad/src/quad_app/callbacks.c | 15 +++-
quad/src/quad_app/control_algorithm.c | 78 +++++++++----------
quad/src/quad_app/log_data.c | 4 +-
30 files changed, 206 insertions(+), 102 deletions(-)
create mode 100644 quad/src/graph_blocks/.gitignore
create mode 100644 quad/src/graph_blocks/Makefile
create mode 100644 quad/src/graph_blocks/graph_blocks.c
create mode 100644 quad/src/graph_blocks/graph_blocks.h
rename quad/src/{computation_graph => graph_blocks}/node_accumulator.c (94%)
rename quad/src/{computation_graph => graph_blocks}/node_accumulator.h (100%)
rename quad/src/{computation_graph => graph_blocks}/node_add.c (94%)
rename quad/src/{computation_graph => graph_blocks}/node_add.h (100%)
rename quad/src/{quad_app => graph_blocks}/node_bounds.c (95%)
rename quad/src/{quad_app => graph_blocks}/node_bounds.h (100%)
rename quad/src/{computation_graph => graph_blocks}/node_constant.c (94%)
rename quad/src/{computation_graph => graph_blocks}/node_constant.h (100%)
rename quad/src/{computation_graph => graph_blocks}/node_gain.c (94%)
rename quad/src/{computation_graph => graph_blocks}/node_gain.h (100%)
rename quad/src/{quad_app => graph_blocks}/node_mixer.c (97%)
rename quad/src/{quad_app => graph_blocks}/node_mixer.h (100%)
rename quad/src/{computation_graph => graph_blocks}/node_mult.c (94%)
rename quad/src/{computation_graph => graph_blocks}/node_mult.h (100%)
rename quad/src/{quad_app => graph_blocks}/node_pid.c (98%)
rename quad/src/{quad_app => graph_blocks}/node_pid.h (100%)
rename quad/src/{computation_graph => graph_blocks}/node_pow.c (94%)
rename quad/src/{computation_graph => graph_blocks}/node_pow.h (100%)
diff --git a/quad/Makefile b/quad/Makefile
index f784e354f..b52debe6e 100644
--- a/quad/Makefile
+++ b/quad/Makefile
@@ -13,6 +13,7 @@ libs:
$(MAKE) -C src/test
$(MAKE) -C src/queue
$(MAKE) -C src/computation_graph
+ $(MAKE) -C src/graph_blocks
$(MAKE) -C src/quad_app
zybo:
@@ -33,6 +34,7 @@ deep-clean:
$(MAKE) -C src/test clean
$(MAKE) -C src/queue clean
$(MAKE) -C src/computation_graph clean
+ $(MAKE) -C src/graph_blocks clean
$(MAKE) -C src/quad_app clean
bash scripts/clean_xsdk_workspace.sh
diff --git a/quad/src/computation_graph/Makefile b/quad/src/computation_graph/Makefile
index 5142ac39b..ddd1f5c2e 100644
--- a/quad/src/computation_graph/Makefile
+++ b/quad/src/computation_graph/Makefile
@@ -1,6 +1,6 @@
TOP=../..
NAME = computation_graph
-REQLIBS = -ltest -lm
+REQLIBS = -ltest -lm -lgraph_blocks
include $(TOP)/library.mk
diff --git a/quad/src/computation_graph/computation_graph.h b/quad/src/computation_graph/computation_graph.h
index 2cd90dc15..8449a903d 100644
--- a/quad/src/computation_graph/computation_graph.h
+++ b/quad/src/computation_graph/computation_graph.h
@@ -25,14 +25,15 @@ struct computation_graph {
// Declares a node type
struct graph_node_type {
- const char* const* input_names;
- const char* const* output_names;
- const char* const* param_names;
- int n_inputs;
- int n_outputs;
- int n_params;
- execute_node_t execute;
- reset_node_t reset;
+ const char* const* input_names; // Array of strings corresponding to the inputs
+ const char* const* output_names; // Array of strings corresponding to the outputs
+ const char* const* param_names; // Array of strings corresponding to the parameters
+ int n_inputs; // Number of inputs
+ int n_outputs; // Number of outputs
+ int n_params; // Number of parameters
+ execute_node_t execute; // Function describing how to produce outputs
+ reset_node_t reset; // Reset this node. Called upon (re)connection
+ size_t state_size; // Size of the state struct for this type
};
// Holds a tuple for defining the source of a node. Includes the node ID and its output ID
diff --git a/quad/src/computation_graph/test/test_computation_graph.c b/quad/src/computation_graph/test/test_computation_graph.c
index 42e913dca..929fe9ca4 100644
--- a/quad/src/computation_graph/test/test_computation_graph.c
+++ b/quad/src/computation_graph/test/test_computation_graph.c
@@ -2,11 +2,7 @@
#include "computation_graph.h"
-#include "node_add.h"
-#include "node_mult.h"
-#include "node_constant.h"
-#include "node_gain.h"
-#include "node_accumulator.h"
+#include "graph_blocks.h"
#define GRAPH_TEST_EPS 0.00001
@@ -19,10 +15,10 @@ static int nequal(double val1, double val2) {
int graph_test_one_add() {
struct computation_graph *graph = create_graph();
- int block = graph_add_node_add(graph, "Add");
- int cblock3 = graph_add_node_const(graph, "3");
+ int block = graph_add_defined_block(graph, BLOCK_ADD, "Add");
+ int cblock3 = graph_add_defined_block(graph, BLOCK_CONSTANT, "3");
graph_set_param_val(graph, cblock3, CONST_SET, 3);
- int cblock4 = graph_add_node_const(graph, "4");
+ int cblock4 = graph_add_defined_block(graph, BLOCK_CONSTANT, "4");
graph_set_param_val(graph, cblock4, CONST_SET, 4);
graph_set_source(graph, block, ADD_SUMMAND1, cblock3, CONST_VAL);
graph_set_source(graph, block, ADD_SUMMAND2, cblock4, CONST_VAL);
@@ -35,8 +31,8 @@ int graph_test_one_add() {
int graph_test_circular_runs() {
struct computation_graph *graph = create_graph();
- int gain1 = graph_add_node_gain(graph, "gain1");
- int gain2 = graph_add_node_gain(graph, "gain2");
+ int gain1 = graph_add_defined_block(graph, BLOCK_GAIN, "gain1");
+ int gain2 = graph_add_defined_block(graph, BLOCK_GAIN, "gain2");
graph_set_source(graph, gain2, GAIN_INPUT, gain1, GAIN_RESULT);
graph_set_source(graph, gain1, GAIN_INPUT, gain2, GAIN_RESULT);
int to_compute_for[1] = {gain2};
@@ -47,8 +43,8 @@ int graph_test_circular_runs() {
int graph_test_circular_resets() {
struct computation_graph *graph = create_graph();
- int acum1 = graph_add_node_accum(graph, "accumulator1");
- int acum2 = graph_add_node_accum(graph, "accumulator2");
+ int acum1 = graph_add_defined_block(graph, BLOCK_ACCUMULATE, "accumulator1");
+ int acum2 = graph_add_defined_block(graph, BLOCK_ACCUMULATE, "accumulator2");
graph_set_source(graph, acum2, ACCUM_IN, acum1, ACCUMULATED);
graph_set_source(graph, acum1, ACCUM_IN, acum2, ACCUMULATED);
return 0; // Passes if no infinite loop
@@ -57,8 +53,8 @@ int graph_test_circular_resets() {
// Tests the accumulator block, thereby testing reset and state changes
int graph_test_accumulator() {
struct computation_graph *graph = create_graph();
- int cblock = graph_add_node_const(graph, "const");
- int acum_b = graph_add_node_accum(graph, "accumulator");
+ int cblock = graph_add_defined_block(graph, BLOCK_CONSTANT, "const");
+ int acum_b = graph_add_defined_block(graph, BLOCK_ACCUMULATE, "accumulator");
graph_set_source(graph, acum_b, ACCUM_IN, cblock, CONST_VAL);
int to_compute_for[1] = {acum_b};
@@ -76,7 +72,7 @@ int graph_test_accumulator() {
}
// Test reset on source set
- int gain_b = graph_add_node_gain(graph, "Gain");
+ int gain_b = graph_add_defined_block(graph, BLOCK_GAIN, "Gain");
graph_set_param_val(graph, gain_b, GAIN_GAIN, 1);
graph_set_source(graph, gain_b, GAIN_INPUT, acum_b, ACCUMULATED);
to_compute_for[0] = gain_b;
@@ -93,9 +89,9 @@ int graph_test_accumulator() {
// even if its output is connected to multiple inputs
int graph_test_single_run() {
struct computation_graph *graph = create_graph();
- int acum_b = graph_add_node_accum(graph, "accumulator");
- int add_block = graph_add_node_add(graph, "Add");
- int cblock = graph_add_node_const(graph, "const");
+ int acum_b = graph_add_defined_block(graph, BLOCK_ACCUMULATE, "accumulator");
+ int add_block = graph_add_defined_block(graph, BLOCK_ADD, "Add");
+ int cblock = graph_add_defined_block(graph, BLOCK_CONSTANT, "const");
graph_set_param_val(graph, cblock, CONST_SET, 2);
@@ -112,10 +108,10 @@ int graph_test_single_run() {
// Tests that upon connection of a second child, a block will not reset
int graph_test_reset_rules() {
struct computation_graph *graph = create_graph();
- int cblock = graph_add_node_const(graph, "5");
+ int cblock = graph_add_defined_block(graph, BLOCK_CONSTANT, "5");
graph_set_param_val(graph, cblock, CONST_SET, 5);
- int acum_b = graph_add_node_accum(graph, "accumulator");
- int gain1 = graph_add_node_gain(graph, "gain1");
+ int acum_b = graph_add_defined_block(graph, BLOCK_ACCUMULATE, "accumulator");
+ int gain1 = graph_add_defined_block(graph, BLOCK_GAIN, "gain1");
graph_set_param_val(graph, gain1, GAIN_GAIN, 1);
graph_set_source(graph, gain1, GAIN_INPUT, acum_b, ACCUMULATED);
@@ -124,7 +120,7 @@ int graph_test_reset_rules() {
graph_compute_nodes(graph, to_compute_for, 1);
// state of acum_b is now 5
- int gain2 = graph_add_node_gain(graph, "gain2");
+ int gain2 = graph_add_defined_block(graph, BLOCK_GAIN, "gain2");
graph_set_param_val(graph, gain2, GAIN_GAIN, 1);
// Connect gain 2, and accumulator should not get reset
graph_set_source(graph, gain2, GAIN_INPUT, acum_b, ACCUMULATED);
@@ -138,7 +134,7 @@ int graph_test_reset_rules() {
int graph_test_self_loop() {
struct computation_graph *graph = create_graph();
- int gain1 = graph_add_node_gain(graph, "gain1");
+ int gain1 = graph_add_defined_block(graph, BLOCK_GAIN, "gain1");
graph_set_source(graph, gain1, GAIN_INPUT, gain1, GAIN_RESULT);
int to_compute_for[1] = {gain1};
graph_compute_nodes(graph, to_compute_for, 1);
@@ -147,8 +143,8 @@ int graph_test_self_loop() {
int graph_test_update_rules() {
struct computation_graph *graph = create_graph();
- int cblock = graph_add_node_const(graph, "const");
- int acum_b = graph_add_node_accum(graph, "accumulator");
+ int cblock = graph_add_defined_block(graph, BLOCK_CONSTANT, "const");
+ int acum_b = graph_add_defined_block(graph, BLOCK_ACCUMULATE, "accumulator");
graph_set_source(graph, acum_b, ACCUM_IN, cblock, CONST_VAL);
graph_set_param_val(graph, cblock, CONST_SET, 3);
@@ -167,11 +163,11 @@ C2 --->| accum_b2 --->|
*/
int graph_test_update_propagation() {
struct computation_graph *graph = create_graph();
- int cblock1 = graph_add_node_const(graph, "const1");
- int cblock2 = graph_add_node_const(graph, "const2");
- int accum_b1 = graph_add_node_accum(graph, "accumulator1");
- int accum_b2 = graph_add_node_accum(graph, "accumulator2");
- int add_b = graph_add_node_add(graph, "add");
+ int cblock1 = graph_add_defined_block(graph, BLOCK_CONSTANT, "const1");
+ int cblock2 = graph_add_defined_block(graph, BLOCK_CONSTANT, "const2");
+ int accum_b1 = graph_add_defined_block(graph, BLOCK_ACCUMULATE, "accumulator1");
+ int accum_b2 = graph_add_defined_block(graph, BLOCK_ACCUMULATE, "accumulator2");
+ int add_b = graph_add_defined_block(graph, BLOCK_ADD, "add");
graph_set_source(graph, accum_b1, ACCUM_IN, cblock1, CONST_VAL);
graph_set_source(graph, accum_b2, ACCUM_IN, cblock2, CONST_VAL);
graph_set_source(graph, add_b, ADD_SUMMAND1, accum_b1, ACCUMULATED);
@@ -199,10 +195,10 @@ the node would never get set.
int graph_test_update_disconnected() {
printf("\n\n---------\n");
struct computation_graph *graph = create_graph();
- int d_block = graph_add_node_const(graph, "const1");
- int gain_block = graph_add_node_gain(graph, "gain");
- int gain2_block = graph_add_node_gain(graph, "gain2");
- int const_b = graph_add_node_const(graph, "const2");
+ int d_block = graph_add_defined_block(graph, BLOCK_CONSTANT, "const1");
+ int gain_block = graph_add_defined_block(graph, BLOCK_GAIN, "gain");
+ int gain2_block = graph_add_defined_block(graph, BLOCK_GAIN, "gain2");
+ int const_b = graph_add_defined_block(graph, BLOCK_CONSTANT, "const2");
graph_set_source(graph, gain_block, GAIN_INPUT, const_b, CONST_VAL);
graph_set_source(graph, gain2_block, GAIN_INPUT, d_block, CONST_VAL); // We need this so d_block doesn't get updated
graph_set_param_val(graph, gain_block, GAIN_GAIN, 2);
@@ -219,8 +215,8 @@ int graph_test_update_disconnected() {
int graph_test_get_source() {
struct computation_graph *graph = create_graph();
- int add_block = graph_add_node_add(graph, "Add");
- int cblock3 = graph_add_node_const(graph, "3");
+ int add_block = graph_add_defined_block(graph, BLOCK_ADD, "Add");
+ int cblock3 = graph_add_defined_block(graph, BLOCK_CONSTANT, "3");
graph_set_source(graph, add_block, ADD_SUMMAND1, cblock3, CONST_VAL);
struct node_src source = graph_get_source(graph, add_block, ADD_SUMMAND1);
@@ -234,7 +230,7 @@ int graph_test_get_source() {
int graph_test_get_source_null() {
struct computation_graph *graph = create_graph();
- int add_block = graph_add_node_add(graph, "Add");
+ int add_block = graph_add_defined_block(graph, BLOCK_ADD, "Add");
struct node_src source = graph_get_source(graph, 123, ADD_SUMMAND1);
if (source.controller_id != -1) {
diff --git a/quad/src/graph_blocks/.gitignore b/quad/src/graph_blocks/.gitignore
new file mode 100644
index 000000000..90edb74c2
--- /dev/null
+++ b/quad/src/graph_blocks/.gitignore
@@ -0,0 +1 @@
+obj/
diff --git a/quad/src/graph_blocks/Makefile b/quad/src/graph_blocks/Makefile
new file mode 100644
index 000000000..2769a61eb
--- /dev/null
+++ b/quad/src/graph_blocks/Makefile
@@ -0,0 +1,6 @@
+TOP=../..
+
+NAME = graph_blocks
+REQLIBS = -lcomputation_graph
+
+include $(TOP)/library.mk
diff --git a/quad/src/graph_blocks/graph_blocks.c b/quad/src/graph_blocks/graph_blocks.c
new file mode 100644
index 000000000..325218bf8
--- /dev/null
+++ b/quad/src/graph_blocks/graph_blocks.c
@@ -0,0 +1,33 @@
+#include "graph_blocks.h"
+#include <stdlib.h>
+
+// See graph_blocks.h
+const struct graph_node_type* blockDefs[MAX_BLOCK_TYPES] =
+{
+ &node_const_type,
+ &node_add_type,
+ &node_mult_type,
+ &node_gain_type,
+ &node_accum_type,
+ &node_bounds_type,
+ &node_mixer_type,
+ &node_pid_type
+};
+
+
+int graph_add_defined_block(struct computation_graph* graph, int type_id, const char* name) {
+ // Verify block type is valid
+ if (type_id >= MAX_BLOCK_TYPES) {
+ return -1;
+ }
+ const struct graph_node_type *block_type = blockDefs[type_id];
+ void* state = NULL;
+ // Allocate the state struct for this node, if necessary
+ if (block_type->state_size) {
+ state = malloc(block_type->state_size);
+ if (!state) {return -1;} // Check for malloc failure
+ }
+
+ // Use the computation graph implementation's add node function
+ return graph_add_node(graph, name, block_type, state);
+}
\ No newline at end of file
diff --git a/quad/src/graph_blocks/graph_blocks.h b/quad/src/graph_blocks/graph_blocks.h
new file mode 100644
index 000000000..7db4f7f21
--- /dev/null
+++ b/quad/src/graph_blocks/graph_blocks.h
@@ -0,0 +1,51 @@
+#include "computation_graph.h"
+#include "node_constant.h"
+#include "node_add.h"
+#include "node_mult.h"
+#include "node_gain.h"
+#include "node_accumulator.h"
+#include "node_bounds.h"
+#include "node_mixer.h"
+#include "node_pid.h"
+
+/*
+ * ---------- How-To ------------
+ * To add a new block type, put the implementation (.c and .h) files
+ * in the same directory as this file, and include the header file above
+ * Add a new entry to this enum right before MAX_BLOCK_TYPES
+ *
+ * In graph_blocks.c, add a new entry at the end of the array with
+ * your graph_node_type struct.
+ */
+
+
+
+/*
+ * Enumerates all the types of different block types.
+ * Must match blockDefs
+ */
+enum BlockTypes {
+ BLOCK_CONSTANT, // 00
+ BLOCK_ADD, // 01
+ BLOCK_MULT, // 02
+ BLOCK_GAIN, // 03
+ BLOCK_ACCUMULATE, // 04
+ BLOCK_BOUNDS, // 05
+ BLOCK_MIXER, // 07
+ BLOCK_PID, // 08
+ // <-- Insert new block type here
+ MAX_BLOCK_TYPES
+};
+
+/*
+ * Array corresponding to the different block type structs
+ * Mustm match the nums in BlockTypes
+ */
+extern const struct graph_node_type* blockDefs[MAX_BLOCK_TYPES];
+
+
+/*
+ * Creates a new node and adds it to the graph with the given type ID and name
+ * Returns the id of the new node upon success, -1 upon failure
+ */
+int graph_add_defined_block(struct computation_graph* graph, int type_id, const char* name);
\ No newline at end of file
diff --git a/quad/src/computation_graph/node_accumulator.c b/quad/src/graph_blocks/node_accumulator.c
similarity index 94%
rename from quad/src/computation_graph/node_accumulator.c
rename to quad/src/graph_blocks/node_accumulator.c
index 17a36ee92..4d02ab374 100644
--- a/quad/src/computation_graph/node_accumulator.c
+++ b/quad/src/graph_blocks/node_accumulator.c
@@ -27,7 +27,8 @@ const struct graph_node_type node_accum_type = {
.n_outputs = 1,
.n_params = 0,
.execute = accum_nodes,
- .reset = reset
+ .reset = reset,
+ .state_size = sizeof(struct accum_state)
};
int graph_add_node_accum(struct computation_graph *graph, const char* name) {
diff --git a/quad/src/computation_graph/node_accumulator.h b/quad/src/graph_blocks/node_accumulator.h
similarity index 100%
rename from quad/src/computation_graph/node_accumulator.h
rename to quad/src/graph_blocks/node_accumulator.h
diff --git a/quad/src/computation_graph/node_add.c b/quad/src/graph_blocks/node_add.c
similarity index 94%
rename from quad/src/computation_graph/node_add.c
rename to quad/src/graph_blocks/node_add.c
index 048a49b3b..db6692856 100644
--- a/quad/src/computation_graph/node_add.c
+++ b/quad/src/graph_blocks/node_add.c
@@ -17,7 +17,8 @@ const struct graph_node_type node_add_type = {
.n_outputs = 1,
.n_params = 0,
.execute = add_nodes,
- .reset = reset
+ .reset = reset,
+ .state_size = 0
};
int graph_add_node_add(struct computation_graph *graph, const char* name) {
diff --git a/quad/src/computation_graph/node_add.h b/quad/src/graph_blocks/node_add.h
similarity index 100%
rename from quad/src/computation_graph/node_add.h
rename to quad/src/graph_blocks/node_add.h
diff --git a/quad/src/quad_app/node_bounds.c b/quad/src/graph_blocks/node_bounds.c
similarity index 95%
rename from quad/src/quad_app/node_bounds.c
rename to quad/src/graph_blocks/node_bounds.c
index aae4a6795..cbcad4a13 100644
--- a/quad/src/quad_app/node_bounds.c
+++ b/quad/src/graph_blocks/node_bounds.c
@@ -25,7 +25,8 @@ const struct graph_node_type node_bounds_type = {
.n_outputs = 1,
.n_params = 2,
.execute = bounds_computation,
- .reset = reset_bounds
+ .reset = reset_bounds,
+ .state_size = 0
};
int graph_add_node_bounds(struct computation_graph *graph, const char* name) {
diff --git a/quad/src/quad_app/node_bounds.h b/quad/src/graph_blocks/node_bounds.h
similarity index 100%
rename from quad/src/quad_app/node_bounds.h
rename to quad/src/graph_blocks/node_bounds.h
diff --git a/quad/src/computation_graph/node_constant.c b/quad/src/graph_blocks/node_constant.c
similarity index 94%
rename from quad/src/computation_graph/node_constant.c
rename to quad/src/graph_blocks/node_constant.c
index b2a46e757..e3ca2188c 100644
--- a/quad/src/computation_graph/node_constant.c
+++ b/quad/src/graph_blocks/node_constant.c
@@ -17,7 +17,8 @@ const struct graph_node_type node_const_type = {
.n_outputs = 1,
.n_params = 1,
.execute = output_const,
- .reset = reset
+ .reset = reset,
+ .state_size = 0
};
int graph_add_node_const(struct computation_graph *graph, const char* name) {
diff --git a/quad/src/computation_graph/node_constant.h b/quad/src/graph_blocks/node_constant.h
similarity index 100%
rename from quad/src/computation_graph/node_constant.h
rename to quad/src/graph_blocks/node_constant.h
diff --git a/quad/src/computation_graph/node_gain.c b/quad/src/graph_blocks/node_gain.c
similarity index 94%
rename from quad/src/computation_graph/node_gain.c
rename to quad/src/graph_blocks/node_gain.c
index 35bb3702c..f16eb2b2a 100644
--- a/quad/src/computation_graph/node_gain.c
+++ b/quad/src/graph_blocks/node_gain.c
@@ -17,7 +17,8 @@ const struct graph_node_type node_gain_type = {
.n_outputs = 1,
.n_params = 1,
.execute = scale_nodes,
- .reset = reset
+ .reset = reset,
+ .state_size = 0
};
int graph_add_node_gain(struct computation_graph *graph, const char* name) {
diff --git a/quad/src/computation_graph/node_gain.h b/quad/src/graph_blocks/node_gain.h
similarity index 100%
rename from quad/src/computation_graph/node_gain.h
rename to quad/src/graph_blocks/node_gain.h
diff --git a/quad/src/quad_app/node_mixer.c b/quad/src/graph_blocks/node_mixer.c
similarity index 97%
rename from quad/src/quad_app/node_mixer.c
rename to quad/src/graph_blocks/node_mixer.c
index 316c1ce82..d7df8cd3a 100644
--- a/quad/src/quad_app/node_mixer.c
+++ b/quad/src/graph_blocks/node_mixer.c
@@ -34,7 +34,8 @@ const struct graph_node_type node_mixer_type = {
.n_outputs = 4,
.n_params = 0,
.execute = mixer_computation,
- .reset = reset_mixer
+ .reset = reset_mixer,
+ .state_size = 0
};
int graph_add_node_mixer(struct computation_graph *graph, const char* name) {
diff --git a/quad/src/quad_app/node_mixer.h b/quad/src/graph_blocks/node_mixer.h
similarity index 100%
rename from quad/src/quad_app/node_mixer.h
rename to quad/src/graph_blocks/node_mixer.h
diff --git a/quad/src/computation_graph/node_mult.c b/quad/src/graph_blocks/node_mult.c
similarity index 94%
rename from quad/src/computation_graph/node_mult.c
rename to quad/src/graph_blocks/node_mult.c
index 2696719fd..ba797a5c3 100644
--- a/quad/src/computation_graph/node_mult.c
+++ b/quad/src/graph_blocks/node_mult.c
@@ -17,7 +17,8 @@ const struct graph_node_type node_mult_type = {
.n_outputs = 1,
.n_params = 0,
.execute = mult_nodes,
- .reset = reset
+ .reset = reset,
+ .state_size = 0
};
int graph_add_node_mult(struct computation_graph *graph, const char* name) {
diff --git a/quad/src/computation_graph/node_mult.h b/quad/src/graph_blocks/node_mult.h
similarity index 100%
rename from quad/src/computation_graph/node_mult.h
rename to quad/src/graph_blocks/node_mult.h
diff --git a/quad/src/quad_app/node_pid.c b/quad/src/graph_blocks/node_pid.c
similarity index 98%
rename from quad/src/quad_app/node_pid.c
rename to quad/src/graph_blocks/node_pid.c
index 722209457..0dc6eed41 100644
--- a/quad/src/quad_app/node_pid.c
+++ b/quad/src/graph_blocks/node_pid.c
@@ -90,7 +90,8 @@ const struct graph_node_type node_pid_type = {
.n_outputs = 1,
.n_params = 4,
.execute = pid_computation,
- .reset = reset_pid
+ .reset = reset_pid,
+ .state_size = sizeof(struct pid_node_state)
};
int graph_add_node_pid(struct computation_graph *graph, const char* name) {
diff --git a/quad/src/quad_app/node_pid.h b/quad/src/graph_blocks/node_pid.h
similarity index 100%
rename from quad/src/quad_app/node_pid.h
rename to quad/src/graph_blocks/node_pid.h
diff --git a/quad/src/computation_graph/node_pow.c b/quad/src/graph_blocks/node_pow.c
similarity index 94%
rename from quad/src/computation_graph/node_pow.c
rename to quad/src/graph_blocks/node_pow.c
index dbbc1d707..65af8e45d 100644
--- a/quad/src/computation_graph/node_pow.c
+++ b/quad/src/graph_blocks/node_pow.c
@@ -18,7 +18,8 @@ const struct graph_node_type node_pow_type = {
.n_outputs = 1,
.n_params = 1,
.execute = pow_nodes,
- .reset = reset
+ .reset = reset,
+ .state_size = 0
};
int graph_add_node_pow(struct computation_graph *graph, const char* name) {
diff --git a/quad/src/computation_graph/node_pow.h b/quad/src/graph_blocks/node_pow.h
similarity index 100%
rename from quad/src/computation_graph/node_pow.h
rename to quad/src/graph_blocks/node_pow.h
diff --git a/quad/src/quad_app/Makefile b/quad/src/quad_app/Makefile
index aea297458..acc452487 100644
--- a/quad/src/quad_app/Makefile
+++ b/quad/src/quad_app/Makefile
@@ -1,6 +1,6 @@
TOP=../..
NAME = quad_app
-REQLIBS = -ltest -lcomputation_graph -lm -lqueue
+REQLIBS = -ltest -lcomputation_graph -lm -lqueue -lgraph_blocks
include $(TOP)/library.mk
diff --git a/quad/src/quad_app/callbacks.c b/quad/src/quad_app/callbacks.c
index f9886913b..c092e20d6 100644
--- a/quad/src/quad_app/callbacks.c
+++ b/quad/src/quad_app/callbacks.c
@@ -303,10 +303,21 @@ int cb_getoutput(modular_structs_t* structs, metadata_t *meta, u8 *data, u16 le
return 0;
}
+/*
+ * Handles a request for the list of nodes in the graph
+ * For N total nodes, returns data in the following format:
+ * |---------------------------------------------------------------------------|
+ * | data index || 0 - 2*N | 2 - 3 | 4 - 5 | 6 - 7 |
+ * |---------------------------------------------------------------------------|
+ * | parameter || Array of node | dest input ID | src node ID | src output ID |
+ * |---------------------------------------------------------------------------|
+ * | bytes || 2 | 2 | 2 | 2 |
+ * |---------------------------------------------------------------------------|
+ */
int cb_getnodes(modular_structs_t* structs, metadata_t *meta, u8 *data, u16 length) {
-
+ return 0;
}
int cb_addnode(modular_structs_t* structs, metadata_t *meta, u8 *data, u16 length) {
-
+ return 0;
}
\ No newline at end of file
diff --git a/quad/src/quad_app/control_algorithm.c b/quad/src/quad_app/control_algorithm.c
index 5e1ebfd7d..178cfc847 100644
--- a/quad/src/quad_app/control_algorithm.c
+++ b/quad/src/quad_app/control_algorithm.c
@@ -8,11 +8,7 @@
// This implemented modular quadrotor software implements a PID control algorithm
#include "control_algorithm.h"
-#include "node_pid.h"
-#include "node_bounds.h"
-#include "node_constant.h"
-#include "node_mixer.h"
-#include "node_add.h"
+#include "graph_blocks.h"
#include "PID.h"
#include "util.h"
#include "timer.h"
@@ -43,55 +39,55 @@ int control_algorithm_init(parameter_t * ps)
ps->graph = graph;
// Create all the PID blocks
- ps->roll_pid = graph_add_node_pid(graph, "Roll PID");
- ps->pitch_pid = graph_add_node_pid(graph, "Pitch PID");
- ps->yaw_pid = graph_add_node_pid(graph, "Yaw PID");
- ps->roll_r_pid = graph_add_node_pid(graph, "Roll Rate PID");
- ps->pitch_r_pid = graph_add_node_pid(graph, "Pitch Rate PID");
- ps->yaw_r_pid = graph_add_node_pid(graph, "Yaw Rate PID");
- ps->x_pos_pid = graph_add_node_pid(graph, "X pos PID");
- ps->y_pos_pid = graph_add_node_pid(graph, "Y pos PID");
- ps->alt_pid = graph_add_node_pid(graph, "Altitude PID");
- ps->x_set = graph_add_node_const(graph, "X Setpoint"); // ID 9
- ps->y_set = graph_add_node_const(graph, "Y Setpoint");
- ps->alt_set = graph_add_node_const(graph, "Alt Setpoint");
- ps->yaw_set = graph_add_node_const(graph, "Yaw Setpoint");
- ps->throttle_trim = graph_add_node_const(graph, "Throttle trim");
- ps->throttle_trim_add = graph_add_node_add(graph, "T trim add");
+ ps->roll_pid = graph_add_defined_block(graph, BLOCK_PID, "Roll PID");
+ ps->pitch_pid = graph_add_defined_block(graph, BLOCK_PID, "Pitch PID");
+ ps->yaw_pid = graph_add_defined_block(graph, BLOCK_PID, "Yaw PID");
+ ps->roll_r_pid = graph_add_defined_block(graph, BLOCK_PID, "Roll Rate PID");
+ ps->pitch_r_pid = graph_add_defined_block(graph, BLOCK_PID, "Pitch Rate PID");
+ ps->yaw_r_pid = graph_add_defined_block(graph, BLOCK_PID, "Yaw Rate PID");
+ ps->x_pos_pid = graph_add_defined_block(graph, BLOCK_PID, "X pos PID");
+ ps->y_pos_pid = graph_add_defined_block(graph, BLOCK_PID, "Y pos PID");
+ ps->alt_pid = graph_add_defined_block(graph, BLOCK_PID, "Altitude PID");
+ ps->x_set = graph_add_defined_block(graph, BLOCK_CONSTANT, "X Setpoint"); // ID 9
+ ps->y_set = graph_add_defined_block(graph, BLOCK_CONSTANT, "Y Setpoint");
+ ps->alt_set = graph_add_defined_block(graph, BLOCK_CONSTANT, "Alt Setpoint");
+ ps->yaw_set = graph_add_defined_block(graph, BLOCK_CONSTANT, "Yaw Setpoint");
+ ps->throttle_trim = graph_add_defined_block(graph, BLOCK_CONSTANT, "Throttle trim");
+ ps->throttle_trim_add = graph_add_defined_block(graph, BLOCK_ADD, "T trim add");
// Create blocks for sensor inputs
- ps->cur_pitch = graph_add_node_const(graph, "Pitch"); // ID 20
- ps->cur_roll = graph_add_node_const(graph, "Roll");
- ps->cur_yaw = graph_add_node_const(graph, "Yaw");
+ ps->cur_pitch = graph_add_defined_block(graph, BLOCK_CONSTANT, "Pitch"); // ID 20
+ ps->cur_roll = graph_add_defined_block(graph, BLOCK_CONSTANT, "Roll");
+ ps->cur_yaw = graph_add_defined_block(graph, BLOCK_CONSTANT, "Yaw");
// Yaw angular velocity PID
// theta_dot is the angular velocity about the y-axis
// phi_dot is the angular velocity about the x-axis
// psi_dot is the angular velocity about the z-axis
// These are calculated from using the gimbal equations
- ps->theta_dot = graph_add_node_const(graph, "dTheta");
- ps->phi_dot = graph_add_node_const(graph, "dPhi");
- ps->psi_dot = graph_add_node_const(graph, "dPsi");
- ps->clamp_d_pwmP = graph_add_node_bounds(graph, "P PWM Clamp");
- ps->clamp_d_pwmR = graph_add_node_bounds(graph, "R PWM Clamp");
- ps->clamp_d_pwmY = graph_add_node_bounds(graph, "Y PWM Clamp");
+ ps->theta_dot = graph_add_defined_block(graph, BLOCK_CONSTANT, "dTheta");
+ ps->phi_dot = graph_add_defined_block(graph, BLOCK_CONSTANT, "dPhi");
+ ps->psi_dot = graph_add_defined_block(graph, BLOCK_CONSTANT, "dPsi");
+ ps->clamp_d_pwmP = graph_add_defined_block(graph, BLOCK_BOUNDS, "P PWM Clamp");
+ ps->clamp_d_pwmR = graph_add_defined_block(graph, BLOCK_BOUNDS, "R PWM Clamp");
+ ps->clamp_d_pwmY = graph_add_defined_block(graph, BLOCK_BOUNDS, "Y PWM Clamp");
// Create blocks for VRPN data
- ps->vrpn_x = graph_add_node_const(graph, "VRPN X");
- ps->vrpn_y = graph_add_node_const(graph, "VRPN Y");
- ps->vrpn_alt = graph_add_node_const(graph, "VRPN Alt");
- ps->vrpn_pitch = graph_add_node_const(graph, "VRPN Pitch");
- ps->vrpn_roll = graph_add_node_const(graph, "VRPN Roll");
+ ps->vrpn_x = graph_add_defined_block(graph, BLOCK_CONSTANT, "VRPN X");
+ ps->vrpn_y = graph_add_defined_block(graph, BLOCK_CONSTANT, "VRPN Y");
+ ps->vrpn_alt = graph_add_defined_block(graph, BLOCK_CONSTANT, "VRPN Alt");
+ ps->vrpn_pitch = graph_add_defined_block(graph, BLOCK_CONSTANT, "VRPN Pitch");
+ ps->vrpn_roll = graph_add_defined_block(graph, BLOCK_CONSTANT, "VRPN Roll");
// Create blocks for RC controller
- ps->rc_pitch = graph_add_node_const(graph, "RC Pitch");
- ps->rc_roll = graph_add_node_const(graph, "RC Roll");
- ps->rc_yaw = graph_add_node_const(graph, "RC Yaw");
- ps->rc_throttle = graph_add_node_const(graph, "RC Throttle");
+ ps->rc_pitch = graph_add_defined_block(graph, BLOCK_CONSTANT, "RC Pitch");
+ ps->rc_roll = graph_add_defined_block(graph, BLOCK_CONSTANT, "RC Roll");
+ ps->rc_yaw = graph_add_defined_block(graph, BLOCK_CONSTANT, "RC Yaw");
+ ps->rc_throttle = graph_add_defined_block(graph, BLOCK_CONSTANT, "RC Throttle");
- ps->mixer = graph_add_node_mixer(graph, "Signal Mixer");
+ ps->mixer = graph_add_defined_block(graph, BLOCK_MIXER, "Signal Mixer");
- ps->angle_time = graph_add_node_const(graph, "Ts_IMU");
- ps->pos_time = graph_add_node_const(graph, "Ts_VRPN");
+ ps->angle_time = graph_add_defined_block(graph, BLOCK_CONSTANT, "Ts_IMU");
+ ps->pos_time = graph_add_defined_block(graph, BLOCK_CONSTANT, "Ts_VRPN");
// Connect pitch PID chain
graph_set_source(graph, ps->pitch_r_pid, PID_SETPOINT, ps->pitch_pid, PID_CORRECTION);
diff --git a/quad/src/quad_app/log_data.c b/quad/src/quad_app/log_data.c
index d7a361dfe..61c9bdf99 100644
--- a/quad/src/quad_app/log_data.c
+++ b/quad/src/quad_app/log_data.c
@@ -14,9 +14,7 @@
#include "log_data.h"
#include "communication.h"
#include "computation_graph.h"
-#include "node_pid.h"
-#include "node_constant.h"
-#include "node_mixer.h"
+#include "graph_blocks.h"
// Current index of the log array
int arrayIndex = 0;
--
GitLab