From e96b487c61d0c23bcaa96a522aa0fb85b980c6ca Mon Sep 17 00:00:00 2001
From: "ucart@co3050-12" <dawehr@iastate.edu>
Date: Fri, 10 Feb 2017 01:10:20 -0600
Subject: [PATCH] Fixed compile issues

---
 .../modular_quad_pid/src/computation_graph.c  |  2 +-
 .../modular_quad_pid/src/computation_graph.h  |  2 +-
 .../modular_quad_pid/src/control_algorithm.c  | 50 +++++++++----------
 .../src/graph_blocks/node_bounds.h            |  2 +-
 .../src/graph_blocks/node_constant.c          | 26 +++++++++-
 .../src/graph_blocks/node_constant.h          | 17 ++++++-
 .../src/graph_blocks/node_pid.c               | 12 +++--
 .../src/graph_blocks/node_pid.h               |  2 +-
 quad/sw/modular_quad_pid/src/type_def.h       | 22 +-------
 9 files changed, 79 insertions(+), 56 deletions(-)
 mode change 120000 => 100644 quad/sw/modular_quad_pid/src/graph_blocks/node_constant.c
 mode change 120000 => 100644 quad/sw/modular_quad_pid/src/graph_blocks/node_constant.h

diff --git a/quad/sw/modular_quad_pid/src/computation_graph.c b/quad/sw/modular_quad_pid/src/computation_graph.c
index b4ff9ed9c..df690a41c 120000
--- a/quad/sw/modular_quad_pid/src/computation_graph.c
+++ b/quad/sw/modular_quad_pid/src/computation_graph.c
@@ -1 +1 @@
-computation_graph/src/computation_graph.c
\ No newline at end of file
+/local/ucart/MicroCART_17-18/quad/computation_graph/src/computation_graph.c
\ No newline at end of file
diff --git a/quad/sw/modular_quad_pid/src/computation_graph.h b/quad/sw/modular_quad_pid/src/computation_graph.h
index b8b749d0f..aa22c46fb 120000
--- a/quad/sw/modular_quad_pid/src/computation_graph.h
+++ b/quad/sw/modular_quad_pid/src/computation_graph.h
@@ -1 +1 @@
-computation_graph/src/computation_graph.h
\ No newline at end of file
+/local/ucart/MicroCART_17-18/quad/computation_graph/src/computation_graph.h
\ No newline at end of file
diff --git a/quad/sw/modular_quad_pid/src/control_algorithm.c b/quad/sw/modular_quad_pid/src/control_algorithm.c
index 757cc8a9b..0e52083f0 100644
--- a/quad/sw/modular_quad_pid/src/control_algorithm.c
+++ b/quad/sw/modular_quad_pid/src/control_algorithm.c
@@ -21,43 +21,43 @@
     ps->graph = graph;
 
     // Create all the PID blocks
-    ps->roll_pid = graph_add_node_pid("Roll PID");
-    ps->pitch_pid = graph_add_node_pid("Pitch PID");
-    ps->yaw_pid = graph_add_node_pid("Yaw PID");
-    ps->roll_r_pid = graph_add_node_pid("Roll Rate PID");
-    ps->pitch_r_pid = graph_add_node_pid("Pitch Rate PID");
-    ps->yaw_r_pid = graph_add_node_pid("Yaw Rate PID");
-    ps->x_pos_pid = graph_add_node_pid("X pos PID PID");
-    ps->y_pos_pid = graph_add_node_pid("Y pos PID");
-    ps->alt_pid = graph_add_node_pid("Altitude");
+    ps->roll_pid = graph_add_node_pid(graph, "Roll PID");
+    ps->pitch_pid = graph_add_node_pid(graph, "Pitch PID");
+    ps->yaw_pid = graph_add_node_pid(graph, "Yaw PID");
+    ps->roll_r_pid = graph_add_node_pid(graph, "Roll Rate PID");
+    ps->pitch_r_pid = graph_add_node_pid(graph, "Pitch Rate PID");
+    ps->yaw_r_pid = graph_add_node_pid(graph, "Yaw Rate PID");
+    ps->x_pos_pid = graph_add_node_pid(graph, "X pos PID PID");
+    ps->y_pos_pid = graph_add_node_pid(graph, "Y pos PID");
+    ps->alt_pid = graph_add_node_pid(graph, "Altitude");
 
     // Create blocks for sensor inputs
-    ps->cur_pitch = graph_add_node_const("Pitch");
-    ps->cur_roll = graph_add_node_const("Roll");
-    ps->cur_yaw = graph_add_node_const("Yaw");
+    ps->cur_pitch = graph_add_node_const(graph, "Pitch");
+    ps->cur_roll = graph_add_node_const(graph, "Roll");
+    ps->cur_yaw = graph_add_node_const(graph, "Yaw");
 	// Yaw angular velocity PID
     // theta_dot is the angular velocity about the y-axis
     // 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("Theta");
-    ps->phi = graph_add_node_const("Phi");
-    ps->psi = graph_add_node_const("Psi");
+    ps->theta = graph_add_node_const(graph, "Theta");
+    ps->phi = graph_add_node_const(graph, "Phi");
+    ps->psi = graph_add_node_const(graph, "Psi");
 
     // Create blocks for RC controller
-    ps->rc_pitch = graph_add_node_const("RC Pitch");
-    ps->rc_roll = graph_add_node_const("RC Roll");
-    ps->rc_yaw = graph_add_node_const("RC Yaw");
-    ps->rc_throttle = graph_add_node_const("RC Throttle");
+    ps->rc_pitch = graph_add_node_const(graph, "RC Pitch");
+    ps->rc_roll = graph_add_node_const(graph, "RC Roll");
+    ps->rc_yaw = graph_add_node_const(graph, "RC Yaw");
+    ps->rc_throttle = graph_add_node_const(graph, "RC Throttle");
 
-    ps->dt = graph_add_node_const("dT");
+    ps->dt = graph_add_node_const(graph, "dT");
 
     // 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_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, 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_DT, ps->dt, CONST_VAL);
 
      // Connect roll PID chain
@@ -65,7 +65,7 @@
     graph_set_source(graph, ps->roll_r_pid, PID_CUR_POINT, ps->phi, 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, 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_DT, ps->dt, CONST_VAL);
 
     // Connect yaw PID chain
@@ -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->cur_theta, CONST_SET, sensor_struct->theta_dot);
-    graph_set_param_val(graph, parameter_struct->cur_phi, CONST_SET, sensor_struct->phi_dot);
-    graph_set_param_val(graph, parameter_struct->cur_psi, CONST_SET, sensor_struct->psi_dot);
+    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->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);
diff --git a/quad/sw/modular_quad_pid/src/graph_blocks/node_bounds.h b/quad/sw/modular_quad_pid/src/graph_blocks/node_bounds.h
index 15415488f..7c8b7e92c 100644
--- a/quad/sw/modular_quad_pid/src/graph_blocks/node_bounds.h
+++ b/quad/sw/modular_quad_pid/src/graph_blocks/node_bounds.h
@@ -1,6 +1,6 @@
 #ifndef __NODE_BOUNDS_H__
 #define __NODE_BOUNDS_H__
-#include "computation_graph.h"
+#include "../computation_graph.h"
 
 int graph_add_node_bounds(struct computation_graph *graph, const char* name);
 
diff --git a/quad/sw/modular_quad_pid/src/graph_blocks/node_constant.c b/quad/sw/modular_quad_pid/src/graph_blocks/node_constant.c
deleted file mode 120000
index 0e47b26d1..000000000
--- a/quad/sw/modular_quad_pid/src/graph_blocks/node_constant.c
+++ /dev/null
@@ -1 +0,0 @@
-computation_graph/src/node_constant.c
\ No newline at end of file
diff --git a/quad/sw/modular_quad_pid/src/graph_blocks/node_constant.c b/quad/sw/modular_quad_pid/src/graph_blocks/node_constant.c
new file mode 100644
index 000000000..47ab571d5
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/graph_blocks/node_constant.c
@@ -0,0 +1,25 @@
+#include "node_constant.h"
+#include <stdlib.h>
+
+static void output_const(void *state, const double *params, const double *inputs, double *outputs) {
+    outputs[CONST_VAL] = params[CONST_SET];
+}
+static void reset(void *state) {}
+
+static const char* const in_names[0] = {};
+static const char* const out_names[1] = {"Constant"};
+static const char* const param_names[1] = {"Constant"};
+const struct graph_node_type node_const_type = {
+        .input_names = in_names,
+        .output_names = out_names,
+        .param_names = param_names,
+        .n_inputs = 0,
+        .n_outputs = 1,
+        .n_params = 1,
+        .execute = output_const,
+        .reset = reset
+};
+
+int graph_add_node_const(struct computation_graph *graph, const char* name) {
+    return graph_add_node(graph, name, &node_const_type, NULL);
+}
diff --git a/quad/sw/modular_quad_pid/src/graph_blocks/node_constant.h b/quad/sw/modular_quad_pid/src/graph_blocks/node_constant.h
deleted file mode 120000
index a231d9af0..000000000
--- a/quad/sw/modular_quad_pid/src/graph_blocks/node_constant.h
+++ /dev/null
@@ -1 +0,0 @@
-computation_graph/src/node_constant.h
\ No newline at end of file
diff --git a/quad/sw/modular_quad_pid/src/graph_blocks/node_constant.h b/quad/sw/modular_quad_pid/src/graph_blocks/node_constant.h
new file mode 100644
index 000000000..91cd6d055
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/graph_blocks/node_constant.h
@@ -0,0 +1,16 @@
+#ifndef __NODE_CONSTANT_H__
+#define __NODE_CONSTANT_H__
+#include "../computation_graph.h"
+
+int graph_add_node_const(struct computation_graph *graph, const char* name);
+
+extern const struct graph_node_type node_const_type;
+
+enum graph_node_const_params {
+    CONST_SET
+};
+
+enum graph_node_const_outputs {
+    CONST_VAL
+};
+#endif //__NODE_CONSTANT_H__
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 745cf0dad..416480c04 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
@@ -1,5 +1,9 @@
 #include "node_pid.h"
 #include <stdlib.h>
+#include <math.h>
+
+static double FLT_EPSILON = 0.0001;
+
 
 struct pid_node_state {
     double prev_error; // Previous error
@@ -39,7 +43,7 @@ static void pid_computation(void *state, const double* params, const double *inp
 
     // Accumulate the error (if Ki is less than epsilon, rougly 0,
     // then reset the accumulated error for safety)
-    if (fabs(pid->Ki) <= FLT_EPSILON) {
+    if (fabs(params[PID_KI]) <= FLT_EPSILON) {
       pid_state->acc_error = 0;
     } else {
       pid_state->acc_error += error;
@@ -49,12 +53,12 @@ static void pid_computation(void *state, const double* params, const double *inp
 
     // Compute each term's contribution
     P = params[PID_KP] * error;
-    I = params[PID_KI] * pid->acc_error * inputs[PID_DT];
+    I = params[PID_KI] * pid_state->acc_error * inputs[PID_DT];
     D = params[PID_KD] * (change_in_error / inputs[PID_DT]);
 
     pid_state->prev_error = error; // Store the current error into the state
 
-    output[PID_CORRECTION] = P + I + D; // Store the computed correction
+    outputs[PID_CORRECTION] = P + I + D; // Store the computed correction
 }
 
 // This function sets the accumulated error and previous error to 0
@@ -82,7 +86,7 @@ const struct graph_node_type node_accum_type = {
 
 int graph_add_node_pid(struct computation_graph *graph, const char* name) {
     struct pid_node_state* node_state = malloc(sizeof(struct pid_node_state));
-    if (sizeof(struct accum_state) && !node_state) {
+    if (sizeof(struct pid_node_state) && !node_state) {
         return -1; // malloc failed
     }
     return graph_add_node(graph, name, &node_accum_type, node_state);
diff --git a/quad/sw/modular_quad_pid/src/graph_blocks/node_pid.h b/quad/sw/modular_quad_pid/src/graph_blocks/node_pid.h
index ce87339b7..75ded4af0 100644
--- a/quad/sw/modular_quad_pid/src/graph_blocks/node_pid.h
+++ b/quad/sw/modular_quad_pid/src/graph_blocks/node_pid.h
@@ -1,6 +1,6 @@
 #ifndef __NODE_PID_H__
 #define __NODE_PID_H__
-#include "computation_graph.h"
+#include "../computation_graph.h"
 
 int graph_add_node_pid(struct computation_graph *graph, const char* name);
 
diff --git a/quad/sw/modular_quad_pid/src/type_def.h b/quad/sw/modular_quad_pid/src/type_def.h
index acc581590..c8af65906 100644
--- a/quad/sw/modular_quad_pid/src/type_def.h
+++ b/quad/sw/modular_quad_pid/src/type_def.h
@@ -1,24 +1,4 @@
-int roll_pid =
-int pitch_pid =
-int yaw_pid = g
-int roll_r_pid
-int pitch_r_pid
-int yaw_r_pid =
-int x_pos_pid =
-int y_pos_pid =
-int alt_pid = g
-// Create block
-int cur_pitch =
-int cur_roll =
-int cur_yaw = g
-int theta = gra
-int phi = graph
-int psi = graph
-// Create block
-int rc_pitch =
-int rc_roll = g
-int rc_yaw = gr
-int rc_throttle/*
+/*
  * struct_def.h
  *
  *  Created on: Mar 2, 2016
-- 
GitLab