Add node to computation graph by ID

Added ability to create nodes with arbitrary IDs. This should only be used if necessary, preferring graph_add_node over graph_add_node_id, because it does not use hashing, and must allocate an array up to the size of the ID you requested.

The ability to add nodes by ID is necessary for the ground station to use the existing graph infrastructure for showing nodes. We could have left the requirement such that after all nodes are added, they must be consecutive, because that is what the quadcopter will return. I am not a fan of this, though, because it seems like something that will bite teams in the future if they don't know about that unexpected behavior.

quad/src/computation_graph/computation_graph.c

@@ -11,9 +11,9 @@ 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 / (8*sizeof(int)))] |=  (1 << (k % (8*sizeof(int)))) )
-#define clearBit(A,k)   ( A[(k / (8*sizeof(int)))] &= ~(1 << (k % (8*sizeof(int)))) )
-#define testBit(A,k)    ( A[(k / (8*sizeof(int)))] &   (1 << (k % (8*sizeof(int)))) )
+#define setBit(A,k)     ( A[(k / (8*sizeof(&A)))] |=  (1 << (k % (8*sizeof(&A)))) )
+#define clearBit(A,k)   ( A[(k / (8*sizeof(&A)))] &= ~(1 << (k % (8*sizeof(&A)))) )
+#define testBit(A,k)    ( A[(k / (8*sizeof(&A)))] &   (1 << (k % (8*sizeof(&A)))) )
 
 struct computation_graph *create_graph() {
     struct computation_graph *the_graph = malloc(sizeof(struct computation_graph));
@@ -74,7 +74,8 @@ int graph_set_source(struct computation_graph *graph,
     }
     struct graph_node *dest_node = &graph->nodes[dest_node_id];
     struct graph_node *src_node = &graph->nodes[src_node_id];
-    if (dest_input >= dest_node->type->n_inputs || src_output >= src_node->type->n_outputs) {
+    if (dest_input >= dest_node->type->n_inputs || src_output >= src_node->type->n_outputs ||
+        dest_input < 0 || src_output < 0) {
         return -1;
     }
 
@@ -182,14 +183,19 @@ double graph_get_output(const struct computation_graph *graph, int node_id, int
     return graph->nodes[node_id].output_values[output_id];
 }
 
+/*
+ * Assumptions: The node passed in is a valid ID (should be checked before passing)
+ * and all node sources are either valid node-output pairs, or the source node ID == -1
+ * This function does not check those assumptions for speed
+*/
 void graph_compute_node_rec(struct computation_graph *graph, int node_id, int depth) {
     if (depth >= GRAPH_MAX_DEPTH) {
-        assert(1 == 0); // TODO :Xil_Assert false
-        return;
-    }
-    if (!graph_node_exists(graph, node_id)) {
+        assert(1 == 0);
         return;
     }
+    // if (!graph_node_exists(graph, node_id)) {
+    //     return;
+    // }
     struct graph_node *node = &graph->nodes[node_id];
     if (node->processed_state != UNPROCESSED) {
         return;
@@ -226,9 +232,8 @@ 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->size; i++) {
-        if (graph_node_exists(graph, i)) {
-            graph->nodes[i].processed_state = UNPROCESSED;
-        }
+        // Note: Do not access malloc'd members in here without first checking if node is valid
+        graph->nodes[i].processed_state = UNPROCESSED;
     }
     for (i = 0; i < n_nodes; i++) {
     	int node_id = node_ids[i];
@@ -238,11 +243,10 @@ void graph_compute_nodes(struct computation_graph *graph, int* node_ids, int n_n
     }
     // Clear all the updated flags for nodes that were actually executed
     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;
-            }
+        // Note: Do not access malloc'd members in here without first checking if node is valid
+        struct graph_node* node = &graph->nodes[i];
+        if (node->processed_state == PROCESSED) {
+            node->updated = 0;
         }
     }
 }