Added ability to get/set PID constants. Passes regression tests, and is...

Added ability to get/set PID constants. Passes regression tests, and is backwards-compatible. Does not support setpoints, because they will be set via a different interface

quad/computation_graph/src/computation_graph.c

@@ -128,6 +128,13 @@ int graph_set_param_val(struct computation_graph *graph, int node_id, int param_
     return 0;
 }
 
+double graph_get_param_val(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) {
+        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) {
         return 0;

quad/computation_graph/src/computation_graph.h

@@ -2,6 +2,7 @@
 #define __COMPUTATION_GRAPH_H__
 
 #include <stdio.h>
+#include <math.h>
 
 typedef void (*execute_node_t)(void *state,
                                const double* params,
@@ -88,6 +89,12 @@ 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);
 
+/*
+ * Returns the value of the param at param_id for the given node
+ * Will return NaN if the given node or parameter IDs are not valid
+ */
+double graph_get_param_val(struct computation_graph *graph, int node_id, int param_id);
+
 /*
  * 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

quad/sw/modular_quad_pid/src/callbacks.c

@@ -2,6 +2,7 @@
 #include "commands.h"
 #include "type_def.h"
 #include "uart.h"
+#include "computation_graph.h"
 
 /*
  * Static variables used to keep track of packet counts
@@ -116,34 +117,14 @@ int cb_setparam(modular_structs_t *structs)
 	{
 		return -1;
 	}
+	struct computation_graph* graph = structs->parameter_struct.graph;
 
-	// Get the controller ID, parameter ID, parameter value
-	u8 controller_id = uart_buff_data_get_u8(0);
+	// Get the node ID, parameter ID, parameter value
+	u8 node_id = uart_buff_data_get_u8(0);
 	u8 param_id = uart_buff_data_get_u8(1);
 	float param_val = uart_buff_data_get_float(2);
-	// Check to make sure the IDs are in bounds
-	if (controller_id >= MAX_CONTROLLER_ID ||
-		param_id >= MAX_CONTROL_PARAM_ID)
-	{
-		return -1;
-	}
-
-	// Set the param_val into the controller by controller_id, param_id
-	switch(param_id)
-	{
-		case KP_ID:
-			structs->parameter_struct.pid_controllers[controller_id].Kp = param_val;
-			break;
-		case KI_ID:
-			structs->parameter_struct.pid_controllers[controller_id].Ki = param_val;
-			break;
-		case KD_ID:
-			structs->parameter_struct.pid_controllers[controller_id].Kd = param_val;
-			break;
-		case SP_ID:
-			structs->parameter_struct.pid_controllers[controller_id].setpoint = param_val;
-			break;
-	}
+	// Set the value for that parameter on that node
+	graph_set_param_val(graph, node_id, param_id, param_val);
 
 	return 0;
 }
@@ -184,39 +165,15 @@ int cb_getparam(modular_structs_t* structs)
 	}
 
 	// Get the controller ID, parameter ID
-	u8 controller_id = uart_buff_data_get_u8(0);
+	u8 node_id = uart_buff_data_get_u8(0);
 	u8 param_id = uart_buff_data_get_u8(1);
-	// Check to make sure the IDs are in bounds
-	if (controller_id >= MAX_CONTROLLER_ID ||
-		param_id >= MAX_CONTROL_PARAM_ID)
-	{
-		return -1;
-	}
-
-	// Make the variable to send
-	float param_val;
-	// Set the param_val equal to the parameter value stored in the controller by
-	// controller_id, param_id
-	switch(param_id)
-	{
-		case KP_ID:
-			param_val = structs->parameter_struct.pid_controllers[controller_id].Kp;
-			break;
-		case KI_ID:
-			param_val = structs->parameter_struct.pid_controllers[controller_id].Ki;
-			break;
-		case KD_ID:
-			param_val = structs->parameter_struct.pid_controllers[controller_id].Kd;
-			break;
-		case SP_ID:
-			param_val = structs->parameter_struct.pid_controllers[controller_id].setpoint;
-			break;
-	}
+	struct computation_graph* graph = structs->parameter_struct.graph;
+	float param_val = graph_get_param_val(graph, node_id, param_id);
 
 	// Format the response data
 	char resp_data[6];
 	// Controller ID
-	resp_data[0] = controller_id;
+	resp_data[0] = node_id;
 	// Parameter ID
 	resp_data[1] = param_id;
 	// Parameter value (4 byte float)

quad/sw/modular_quad_pid/src/initialize_components.c

@@ -8,7 +8,7 @@
 #include "initialize_components.h"
 #include "communication.h"
 
-//#define BENCH_TEST
+#define BENCH_TEST
 
 extern int Xil_AssertWait;
 

quad/sw/modular_quad_pid/src/main.c

@@ -16,7 +16,7 @@
 #include "send_actuator_commands.h"
 #include "update_gui.h"
 
-//#define BENCH_TEST
+#define BENCH_TEST
 //#define UART_BENCHMARK
 
 int main()