From 211c5385537e0bbe97ec88d72e1754653d82b380 Mon Sep 17 00:00:00 2001
From: "ucart@co3050-12" <dawehr@iastate.edu>
Date: Sat, 11 Feb 2017 20:11:58 -0600
Subject: [PATCH] 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
---
.../computation_graph/src/computation_graph.c | 7 +++
.../computation_graph/src/computation_graph.h | 7 +++
quad/sw/modular_quad_pid/src/callbacks.c | 63 +++----------------
.../src/initialize_components.c | 2 +-
quad/sw/modular_quad_pid/src/main.c | 2 +-
5 files changed, 26 insertions(+), 55 deletions(-)
diff --git a/quad/computation_graph/src/computation_graph.c b/quad/computation_graph/src/computation_graph.c
index 48c86467d..caf7a593c 100644
--- a/quad/computation_graph/src/computation_graph.c
+++ b/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;
diff --git a/quad/computation_graph/src/computation_graph.h b/quad/computation_graph/src/computation_graph.h
index 797832301..946ffa67d 100644
--- a/quad/computation_graph/src/computation_graph.h
+++ b/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
diff --git a/quad/sw/modular_quad_pid/src/callbacks.c b/quad/sw/modular_quad_pid/src/callbacks.c
index 0ae4320d0..2fb49c0f4 100644
--- a/quad/sw/modular_quad_pid/src/callbacks.c
+++ b/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)
diff --git a/quad/sw/modular_quad_pid/src/initialize_components.c b/quad/sw/modular_quad_pid/src/initialize_components.c
index c548d26d2..c83ba76f0 100644
--- a/quad/sw/modular_quad_pid/src/initialize_components.c
+++ b/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;
diff --git a/quad/sw/modular_quad_pid/src/main.c b/quad/sw/modular_quad_pid/src/main.c
index cfc3076a8..497d334bc 100644
--- a/quad/sw/modular_quad_pid/src/main.c
+++ b/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()
--
GitLab