From 1daf19721f2e77104fb0aabd0e763f12b58b9272 Mon Sep 17 00:00:00 2001
From: "ucart@co3050-12" <dawehr@iastate.edu>
Date: Fri, 10 Feb 2017 02:27:43 -0600
Subject: [PATCH] Fixed a few bugs. still not working
---
.../computation_graph/src/computation_graph.c | 20 +++++++++----
.../computation_graph/src/computation_graph.h | 6 ++--
.../modular_quad_pid/src/control_algorithm.c | 29 ++++++++++---------
.../src/graph_blocks/node_pid.c | 8 ++---
quad/sw/modular_quad_pid/src/type_def.h | 6 ++--
5 files changed, 39 insertions(+), 30 deletions(-)
diff --git a/quad/computation_graph/src/computation_graph.c b/quad/computation_graph/src/computation_graph.c
index 91e778d25..c2f0df661 100644
--- a/quad/computation_graph/src/computation_graph.c
+++ b/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
diff --git a/quad/computation_graph/src/computation_graph.h b/quad/computation_graph/src/computation_graph.h
index bf30a70c2..797832301 100644
--- a/quad/computation_graph/src/computation_graph.h
+++ b/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
diff --git a/quad/sw/modular_quad_pid/src/control_algorithm.c b/quad/sw/modular_quad_pid/src/control_algorithm.c
index 0e52083f0..4def3b2fa 100644
--- a/quad/sw/modular_quad_pid/src/control_algorithm.c
+++ b/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);
diff --git a/quad/sw/modular_quad_pid/src/graph_blocks/node_pid.c b/quad/sw/modular_quad_pid/src/graph_blocks/node_pid.c
index 416480c04..acf77b5b4 100644
--- a/quad/sw/modular_quad_pid/src/graph_blocks/node_pid.c
+++ b/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);
}
diff --git a/quad/sw/modular_quad_pid/src/type_def.h b/quad/sw/modular_quad_pid/src/type_def.h
index c8af65906..6bbe413f4 100644
--- a/quad/sw/modular_quad_pid/src/type_def.h
+++ b/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;
--
GitLab