Fixed a few bugs. still not working

quad/computation_graph/src/computation_graph.c

@@ -169,12 +169,17 @@ void graph_compute_node_rec(struct computation_graph *graph, int node_id, int de
     node->processed_state = PROCESSED;
 }
 
-void graph_compute_node(struct computation_graph *graph, int node_id) {
+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;
     }
-    graph_compute_node_rec(graph, node_id, 0);
+    for (i = 0; i < n_nodes; i++) {
+    	int node_id = node_ids[i];
+    	if (node_id < graph->n_nodes) {
+    	    graph_compute_node_rec(graph, node_id, 0);
+    	}
+    }
 }
 
 int export_dot(const struct computation_graph* graph, FILE* of) {
@@ -200,10 +205,13 @@ int export_dot(const struct computation_graph* graph, FILE* of) {
         fprintf(of, "\"]\n"); // Close label bracket
         // Make connections from
         for (j = 0; j < node->type->n_inputs; j++) {
-            struct graph_node* src_node = &graph->nodes[node->input_srcs[j].controller_id];
-            int output_id = node->input_srcs[j].controller_output;
-            const char* output_name = src_node->type->output_names[output_id];
-            fprintf(of, "\"%s\" -> \"%s\":f%d [label=\"%s=%.3f\"]\n", src_node->name, node->name, j+1, output_name, src_node->output_values[output_id]);
+        	int input_id = node->input_srcs[j].controller_id;
+        	if (input_id != -1) {
+                struct graph_node* src_node = &graph->nodes[input_id];
+                int output_id = node->input_srcs[j].controller_output;
+                const char* output_name = src_node->type->output_names[output_id];
+                fprintf(of, "\"%s\" -> \"%s\":f%d [label=\"%s=%.3f\"]\n", src_node->name, node->name, j+1, output_name, src_node->output_values[output_id]);
+        	}
         }
     }
     fprintf(of, "}"); // Close graph

quad/computation_graph/src/computation_graph.h

@@ -89,11 +89,11 @@ double graph_get_output(const struct computation_graph *graph, int node_id, int
 int graph_set_param_val(struct computation_graph *graph, int node_id, int param_id, double value);
 
 /*
- * Computes the node given by node_id.
- * To do so, computes all nodes which are ancestors of node_id in topological order, with
+ * Computes the nodes given by node_id.
+ * To do so, computes all nodes which are ancestors of each given node_id in topological order, with
  * the final computation being node_id itself.
  */
-void graph_compute_node(struct computation_graph *graph, int node_id);
+void graph_compute_nodes(struct computation_graph *graph, int* node_ids, int n_nodes);
 
 /*
  * Writes a graphical representation of the given graph to <of> in the DOT language

quad/sw/modular_quad_pid/src/control_algorithm.c

@@ -40,9 +40,9 @@
     // 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 = graph_add_node_const(graph, "Theta");
-    ps->phi = graph_add_node_const(graph, "Phi");
-    ps->psi = graph_add_node_const(graph, "Psi");
+    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");
 
     // Create blocks for RC controller
     ps->rc_pitch = graph_add_node_const(graph, "RC Pitch");
@@ -54,23 +54,23 @@
 
     // Connect pitch PID chain
     graph_set_source(graph, ps->pitch_r_pid, PID_SETPOINT, ps->pitch_pid, PID_CORRECTION);
-    graph_set_source(graph, ps->pitch_r_pid, PID_CUR_POINT, ps->theta, CONST_VAL);
+    graph_set_source(graph, ps->pitch_r_pid, PID_CUR_POINT, ps->theta_dot, CONST_VAL);
     graph_set_source(graph, ps->pitch_r_pid, PID_DT, ps->dt, CONST_VAL);
     graph_set_source(graph, ps->pitch_pid, PID_SETPOINT, ps->rc_pitch, CONST_VAL);
-    graph_set_source(graph, ps->pitch_pid, PID_CUR_POINT, ps->pitch_pid, CONST_VAL);
+    graph_set_source(graph, ps->pitch_pid, PID_CUR_POINT, ps->cur_pitch, CONST_VAL);
     graph_set_source(graph, ps->pitch_pid, PID_DT, ps->dt, CONST_VAL);
 
      // Connect roll PID chain
     graph_set_source(graph, ps->roll_r_pid, PID_SETPOINT, ps->roll_pid, PID_CORRECTION);
-    graph_set_source(graph, ps->roll_r_pid, PID_CUR_POINT, ps->phi, CONST_VAL);
+    graph_set_source(graph, ps->roll_r_pid, PID_CUR_POINT, ps->phi_dot, CONST_VAL);
     graph_set_source(graph, ps->roll_r_pid, PID_DT, ps->dt, CONST_VAL);
     graph_set_source(graph, ps->roll_pid, PID_SETPOINT, ps->rc_roll, CONST_VAL);
-    graph_set_source(graph, ps->roll_pid, PID_CUR_POINT, ps->roll_pid, CONST_VAL);
+    graph_set_source(graph, ps->roll_pid, PID_CUR_POINT, ps->cur_roll, CONST_VAL);
     graph_set_source(graph, ps->roll_pid, PID_DT, ps->dt, CONST_VAL);
 
     // Connect yaw PID chain
     graph_set_source(graph, ps->yaw_r_pid, PID_SETPOINT, ps->rc_yaw, PID_CORRECTION);
-    graph_set_source(graph, ps->yaw_r_pid, PID_CUR_POINT, ps->psi, CONST_VAL);
+    graph_set_source(graph, ps->yaw_r_pid, PID_CUR_POINT, ps->psi_dot, CONST_VAL);
     graph_set_source(graph, ps->yaw_r_pid, PID_DT, ps->dt, CONST_VAL);
     /*
     graph_set_source(graph, ps->yaw_pid, PID_SETPOINT, ps->rc_yaw, CONST_VAL);
@@ -219,9 +219,9 @@
 
     graph_set_param_val(graph, parameter_struct->cur_pitch, CONST_SET, sensor_struct->pitch_angle_filtered);
     graph_set_param_val(graph, parameter_struct->cur_roll, CONST_SET, sensor_struct->roll_angle_filtered);
-    graph_set_param_val(graph, parameter_struct->theta, CONST_SET, sensor_struct->theta_dot);
-    graph_set_param_val(graph, parameter_struct->phi, CONST_SET, sensor_struct->phi_dot);
-    graph_set_param_val(graph, parameter_struct->psi, CONST_SET, sensor_struct->psi_dot);
+    graph_set_param_val(graph, parameter_struct->theta_dot, CONST_SET, sensor_struct->theta_dot);
+    graph_set_param_val(graph, parameter_struct->phi_dot, CONST_SET, sensor_struct->phi_dot);
+    graph_set_param_val(graph, parameter_struct->psi_dot, CONST_SET, sensor_struct->psi_dot);
 
     graph_set_param_val(graph, parameter_struct->rc_pitch, CONST_SET, user_input_struct->pitch_angle_manual_setpoint);
     graph_set_param_val(graph, parameter_struct->rc_roll, CONST_SET, user_input_struct->roll_angle_manual_setpoint);
@@ -237,9 +237,10 @@
 			(parameter_struct->pid_controllers[LOCAL_Y_ID].pid_correction) + roll_trim : user_input_struct->roll_angle_manual_setpoint;
     */
 
-    graph_compute_node(graph, parameter_struct->pitch_r_pid);
-    graph_compute_node(graph, parameter_struct->roll_r_pid);
-    graph_compute_node(graph, parameter_struct->yaw_r_pid);
+    int outputs[3] = {parameter_struct->pitch_r_pid,
+                             parameter_struct->roll_r_pid,
+                             parameter_struct->yaw_r_pid};
+    graph_compute_nodes(graph, outputs, 3);
 
 	 // here for now so in case any flight command is not PID controlled, it will default to rc_command value:
 	memcpy(raw_actuator_struct->controller_corrected_motor_commands, user_input_struct->rc_commands, sizeof(int) * 6);

quad/sw/modular_quad_pid/src/graph_blocks/node_pid.c

@@ -34,7 +34,7 @@ struct pid_node_state {
 //																				 ------------------------
 //
 static void pid_computation(void *state, const double* params, const double *inputs, double *outputs) {
-    struct pid_node_state* pid_state = (struct pid_state*)state;
+    struct pid_node_state* pid_state = (struct pid_node_state*)state;
 
     double P = 0.0, I = 0.0, D = 0.0;
 
@@ -64,7 +64,7 @@ static void pid_computation(void *state, const double* params, const double *inp
 // This function sets the accumulated error and previous error to 0
 // to prevent previous errors from affecting output after a reset
 static void reset_pid(void *state) {
-    struct pid_node_state* pid_state = (struct pid_state*)state;
+    struct pid_node_state* pid_state = (struct pid_node_state*)state;
     pid_state->acc_error = 0;
     pid_state->prev_error = 0;
 }
@@ -73,7 +73,7 @@ static void reset_pid(void *state) {
 static const char* const in_names[3] = {"Cur point", "Setpoint", "dt"};
 static const char* const out_names[1] = {"Correction"};
 static const char* const param_names[3] = {"Kp", "Ki", "Kd"};
-const struct graph_node_type node_accum_type = {
+const struct graph_node_type node_pid_type = {
         .input_names = in_names,
         .output_names = out_names,
         .param_names = param_names,
@@ -89,5 +89,5 @@ int graph_add_node_pid(struct computation_graph *graph, const char* name) {
     if (sizeof(struct pid_node_state) && !node_state) {
         return -1; // malloc failed
     }
-    return graph_add_node(graph, name, &node_accum_type, node_state);
+    return graph_add_node(graph, name, &node_pid_type, node_state);
 }

quad/sw/modular_quad_pid/src/type_def.h

@@ -304,9 +304,9 @@ typedef struct parameter_t {
 	int cur_pitch;
 	int cur_roll;
 	int cur_yaw;
-	int theta;
-	int phi;
-	int psi;
+	int theta_dot;
+	int phi_dot;
+	int psi_dot;
 	// RC blocks
 	int rc_pitch;
 	int rc_roll;