diff --git a/quad/src/computation_graph/computation_graph.c b/quad/src/computation_graph/computation_graph.c
index 7b21b2273e111b96160293108fa08aed2b7de5d5..8720fcb9851edaea00864533d852dfb7653eee95 100644
--- a/quad/src/computation_graph/computation_graph.c
+++ b/quad/src/computation_graph/computation_graph.c
@@ -9,11 +9,19 @@
// Array to store input values for passing to the execute function of each node
static double exec_input_vals[GRAPH_MAX_INPUTS];
+// Macro functions for setting and clearing single bits in int array
+// From http://www.mathcs.emory.edu/~cheung/Courses/255/Syllabus/1-C-intro/bit-array.html
+#define setBit(A,k) ( A[(k / sizeof(int))] |= (1 << (k % sizeof(int))) )
+#define clearBit(A,k) ( A[(k / sizeof(int))] &= ~(1 << (k % sizeof(int))) )
+#define testBit(A,k) ( A[(k / sizeof(int))] & (1 << (k % sizeof(int))) )
+
struct computation_graph *create_graph() {
struct computation_graph *the_graph = malloc(sizeof(struct computation_graph));
if (!the_graph) {return NULL;}
+ // Allocate space for a single node in the graph
the_graph->nodes = malloc(sizeof(struct graph_node));
- if (!the_graph->nodes) { return NULL; }
+ the_graph->node_existence = malloc(sizeof(int));
+ if (!the_graph->nodes || !the_graph->node_existence) { return NULL; }
the_graph->n_nodes = 0;
the_graph->size = 1;
return the_graph;
@@ -46,12 +54,14 @@ static void reset_node_rec(struct computation_graph* graph, int node_id, int dep
}
int reset_node(struct computation_graph* graph, int node_id) {
- if (node_id >= graph->n_nodes) {
+ if (!graph_node_exists(graph, node_id)) {
return -1;
}
int i;
- for (i = 0; i < graph->n_nodes; i++) {
- graph->nodes[i].processed_state = UNPROCESSED;
+ for (i = 0; i < graph->size; i++) {
+ if (graph_node_exists(graph, i)) {
+ graph->nodes[i].processed_state = UNPROCESSED;
+ }
}
reset_node_rec(graph, node_id, 0);
return 0;
@@ -59,7 +69,7 @@ int reset_node(struct computation_graph* graph, int node_id) {
int graph_set_source(struct computation_graph *graph,
int dest_node_id, int dest_input, int src_node_id, int src_output) {
- if (dest_node_id >= graph->n_nodes || src_node_id >= graph->n_nodes) {
+ if (!graph_node_exists(graph, dest_node_id) || !graph_node_exists(graph, src_node_id)) {
return -1;
}
struct graph_node *dest_node = &graph->nodes[dest_node_id];
@@ -82,7 +92,7 @@ int graph_set_source(struct computation_graph *graph,
}
struct node_src graph_get_source(struct computation_graph *graph, int node_id, int input_id) {
- if (node_id >= graph->n_nodes || node_id < 0 ||
+ if (!graph_node_exists(graph, node_id) ||
input_id >= graph->nodes[node_id].type->n_inputs || input_id < 0) {
return (struct node_src) {.controller_id = -1, .controller_output = -1};
}
@@ -95,16 +105,33 @@ int graph_add_node(struct computation_graph *graph,
void *state) {
assert(type->n_inputs <= GRAPH_MAX_INPUTS);
int new_id = graph->n_nodes;
- if (new_id >= graph->size) {
- int new_capacity = graph->n_nodes == 0 ? 1 : graph->n_nodes * 2;
- struct graph_node *node_arr = realloc(graph->nodes, sizeof(struct graph_node) * new_capacity);
- if (!node_arr) {
- return -1;
+ return graph_add_node_id(graph, new_id, name, type, state);
+}
+
+int graph_add_node_id(struct computation_graph *graph,
+ int id,
+ const char *name,
+ const struct graph_node_type *type,
+ void *state) {
+ if (id >= graph->size) {
+ size_t old_size = graph->size;
+ size_t new_size = old_size == 0 ? 8 : id * 2; // Hold twice the given ID
+ struct graph_node *node_arr = realloc(graph->nodes, sizeof(struct graph_node) * new_size);
+ if (!node_arr) { return -1; }
+ // Number of integers needed to hold new_size bits
+ size_t new_exist_size = ceil((float)new_size / (8 * sizeof(int))); // ceil(new_size / (bits per int))
+ int* exist_arr = realloc(graph->node_existence, sizeof(int) * new_exist_size);
+ if (!exist_arr) {return -1;}
+ // Set the newly allocated memory to 0
+ size_t old_exist_size = ceil((float)old_size / (8 * sizeof(int)));
+ if (old_exist_size != new_exist_size) {
+ memset(exist_arr + old_exist_size, 0, (new_exist_size - old_exist_size) * sizeof(int));
}
- graph->size = new_capacity;
+ graph->size = new_size;
graph->nodes = node_arr;
+ graph->node_existence = exist_arr;
}
- struct graph_node *new_node = &graph->nodes[new_id];
+ struct graph_node *new_node = &graph->nodes[id];
new_node->name = strdup(name);
new_node->type = type;
new_node->state = state;
@@ -124,15 +151,16 @@ int graph_add_node(struct computation_graph *graph,
new_node->input_srcs[i].controller_id = -1;
}
graph->n_nodes += 1;
+ setBit(graph->node_existence, id);
// Reset block upon creation
if (new_node->type->reset != NULL) {
new_node->type->reset(new_node->state);
}
- return new_id;
+ return id;
}
int graph_set_param_val(struct computation_graph *graph, int node_id, int param_id, double value) {
- if (node_id >= graph->n_nodes || param_id >= graph->nodes[node_id].type->n_params) {
+ if (!graph_node_exists(graph, node_id) || param_id >= graph->nodes[node_id].type->n_params) {
return -1;
}
graph->nodes[node_id].param_values[param_id] = value;
@@ -141,14 +169,14 @@ int graph_set_param_val(struct computation_graph *graph, int node_id, int param_
}
double graph_get_param_val(const struct computation_graph *graph, int node_id, int param_id) {
- if (node_id >= graph->n_nodes || param_id >= graph->nodes[node_id].type->n_params) {
+ if (!graph_node_exists(graph, node_id) || param_id >= graph->nodes[node_id].type->n_params) {
return NAN;
}
return graph->nodes[node_id].param_values[param_id];
}
double graph_get_output(const struct computation_graph *graph, int node_id, int output_id) {
- if (node_id >= graph->n_nodes || output_id >= graph->nodes[node_id].type->n_outputs) {
+ if (!graph_node_exists(graph, node_id) || output_id >= graph->nodes[node_id].type->n_outputs) {
return NAN;
}
return graph->nodes[node_id].output_values[output_id];
@@ -159,7 +187,7 @@ void graph_compute_node_rec(struct computation_graph *graph, int node_id, int de
assert(1 == 0); // TODO :Xil_Assert false
return;
}
- if (node_id >= graph->n_nodes) {
+ if (!graph_node_exists(graph, node_id)) {
return;
}
struct graph_node *node = &graph->nodes[node_id];
@@ -197,20 +225,24 @@ void graph_compute_node_rec(struct computation_graph *graph, int node_id, int de
void graph_compute_nodes(struct computation_graph *graph, int* node_ids, int n_nodes) {
int i;
- for (i = 0; i < graph->n_nodes; i++) {
- graph->nodes[i].processed_state = UNPROCESSED;
+ for (i = 0; i < graph->size; i++) {
+ if (graph_node_exists(graph, i)) {
+ graph->nodes[i].processed_state = UNPROCESSED;
+ }
}
for (i = 0; i < n_nodes; i++) {
int node_id = node_ids[i];
- if (node_id < graph->n_nodes) {
+ if (graph_node_exists(graph, node_id)) {
graph_compute_node_rec(graph, node_id, 0);
}
}
// Clear all the updated flags for nodes that were actually executed
- for (i = 0; i < graph->n_nodes; i++) {
- struct graph_node* node = &graph->nodes[i];
- if (node->processed_state == PROCESSED) {
- node->updated = 0;
+ for (i = 0; i < graph->size; i++) {
+ if (graph_node_exists(graph, i)) {
+ struct graph_node* node = &graph->nodes[i];
+ if (node->processed_state == PROCESSED) {
+ node->updated = 0;
+ }
}
}
}
@@ -221,7 +253,8 @@ int export_dot(const struct computation_graph* graph, FILE* of, int print_output
// Draw all the nodes and their inputs
int i;
- for (i = 0; i < graph->n_nodes; i++) {
+ for (i = 0; i < graph->size; i++) {
+ if (!graph_node_exists(graph, i)) {continue;}
struct graph_node *node = &graph->nodes[i];
// Create node
fprintf(of, "\"%s\"[shape=record\nlabel=\"", node->name);
@@ -254,3 +287,10 @@ int export_dot(const struct computation_graph* graph, FILE* of, int print_output
fprintf(of, "}"); // Close graph
return 0;
}
+
+int graph_node_exists(const struct computation_graph *graph, int node_id) {
+ if (node_id < 0 || node_id >= graph->size || !testBit(graph->node_existence, node_id)) {
+ return 0;
+ }
+ else {return 1;}
+}
diff --git a/quad/src/computation_graph/computation_graph.h b/quad/src/computation_graph/computation_graph.h
index e997aa7ab06a79d61aac152699d9a49379403ffc..adddf5119ff742f9f2a603936c8d3b59ae4c6deb 100644
--- a/quad/src/computation_graph/computation_graph.h
+++ b/quad/src/computation_graph/computation_graph.h
@@ -21,6 +21,7 @@ struct computation_graph {
int n_nodes;
int size;
struct graph_node *nodes;
+ int* node_existence; // Single-bit values indicating whether a node with a particular ID exists
};
// Declares a node type
@@ -89,6 +90,18 @@ int graph_add_node(struct computation_graph *graph,
const struct graph_node_type *type,
void *state);
+/*
+ * Similar to graph_add_node, but adds with a specific ID
+ * WARNING: Do not try to use this to create nodes with arbitrary IDs,
+ * as it stores IDs sequentially in an array, so a large ID will result
+ * in at least that many elements being allocated
+ */
+int graph_add_node_id(struct computation_graph *graph,
+ int id,
+ const char *name,
+ const struct graph_node_type *type,
+ void *state);
+
/*
* Returns the value at the output of the requested node for the requested output.
* Returns 0 if the given node or output IDs are invalid
@@ -114,6 +127,12 @@ double graph_get_param_val(const struct computation_graph *graph, int node_id, i
*/
void graph_compute_nodes(struct computation_graph *graph, int* node_ids, int n_nodes);
+/*
+ * Check if a particular node with a given ID has been added
+ * Returns 1 if node exists, 0 otherwise
+ */
+int graph_node_exists(const struct computation_graph *graph, int node_id);
+
/*
* Writes a graphical representation of the given graph to <of> in the DOT language
*/
diff --git a/quad/src/computation_graph/test/test_computation_graph.c b/quad/src/computation_graph/test/test_computation_graph.c
index 929fe9ca44404efbaa9e46287cc78a6a2fe057f9..b0b6a14505ce0ca581f02ff82a2d1d010efe42b5 100644
--- a/quad/src/computation_graph/test/test_computation_graph.c
+++ b/quad/src/computation_graph/test/test_computation_graph.c
@@ -241,6 +241,28 @@ int graph_test_get_source_null() {
}
}
+int graph_test_add_by_id() {
+ struct computation_graph *graph = create_graph();
+ int desired_id = 87;
+ int add_block = graph_add_node_id(graph, desired_id, "Add", &node_add_type, NULL);
+ if (add_block != desired_id) {
+ return -1;
+ }
+ int const1 = graph_add_node_id(graph, 12, "const1", &node_const_type, NULL);
+ graph_set_param_val(graph, const1, CONST_SET, 3.5);
+ int const2 = graph_add_node_id(graph, 123, "const2", &node_const_type, NULL);
+ graph_set_param_val(graph, const2, CONST_SET, 2.5);
+ graph_set_source(graph, add_block, ADD_SUMMAND1, const1, CONST_VAL);
+ graph_set_source(graph, add_block, ADD_SUMMAND2, const2, CONST_VAL);
+
+ int to_compute_for[] = {add_block};
+ graph_compute_nodes(graph, to_compute_for, 1);
+ double result = graph_get_output(graph, add_block, ADD_SUM);
+ printf("n_nodes: %d, size: %d\n", graph->n_nodes, graph->size);
+ printf("result: %f", result);
+ return nequal(result, 3.5 + 2.5);
+}
+
int main() {
test(graph_test_one_add, "Test adding 2 numbers");
test(graph_test_circular_runs, "Test computing cycles");
@@ -254,5 +276,6 @@ int main() {
test(graph_test_update_disconnected, "Tests that nodes get executed when updated, even if disconnected");
test(graph_test_get_source, "Tests that the get_source call works normally");
test(graph_test_get_source_null, "Tests that the get_source call returns ID -1 when invalid ID is passed");
+ test(graph_test_add_by_id, "Tests that new nodes can be created by ID");
return test_summary();
}
diff --git a/quad/src/quad_app/log_data.c b/quad/src/quad_app/log_data.c
index a7cba2c85d43cc8b1bf51ca3f3d1665f46e03e89..5ca0d16a65936d6ca8211213cd9549005f9f9c1a 100644
--- a/quad/src/quad_app/log_data.c
+++ b/quad/src/quad_app/log_data.c
@@ -17,9 +17,9 @@
#include "graph_blocks.h"
// Current index of the log array
-int arrayIndex = 0;
+static int arrayIndex = 0;
// Size of the array
-int arraySize = 0;
+static int arraySize = 0;
struct graph_tuple { // Tuple for
int block_id;
@@ -42,11 +42,11 @@ struct str {
struct graph_tuple log_outputs[MAX_LOG_NUM];
struct graph_tuple log_params[MAX_LOG_NUM];
-size_t n_outputs;
-size_t n_params;
+static size_t n_outputs;
+static size_t n_params;
-float* logArray = NULL;
-int row_size;
+static float* logArray = NULL;
+static int row_size;
static char units_output_str[512] = {};
static char units_param_str[512] = {};