diff --git a/quad/computation_graph/src/computation_graph.c b/quad/computation_graph/src/computation_graph.c
index 412c1d49e436b8093639a0e4b5d1147c2ead5ce8..2204f3a586be0ce976b946b6a826bc9726c8a5fe 100644
--- a/quad/computation_graph/src/computation_graph.c
+++ b/quad/computation_graph/src/computation_graph.c
@@ -221,11 +221,11 @@ int export_dot(const struct computation_graph* graph, FILE* of, int print_output
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", src_node->name, node->name, j+1);
+ fprintf(of, "\"%s\" -> \"%s\":f%d [label=\"%s", src_node->name, node->name, j+1, output_name);
if (print_outputs) {
- fprintf(of, " [label=\"%s=%.3f\"]", output_name, src_node->output_values[output_id]);
+ fprintf(of, "=%.3f", src_node->output_values[output_id]);
}
- fprintf(of, "\n");
+ fprintf(of, "\"]\n");
}
}
}
diff --git a/quad/sw/modular_quad_pid/.cproject b/quad/sw/modular_quad_pid/.cproject
index c203f31db275e586aceec2167d98420327afe1ed..2a974bb61e6abb6f71e0b057d0beb3c3c2a5b789 100644
--- a/quad/sw/modular_quad_pid/.cproject
+++ b/quad/sw/modular_quad_pid/.cproject
@@ -71,7 +71,7 @@
</toolChain>
</folderInfo>
<sourceEntries>
- <entry excluding="test" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/>
+ <entry excluding="gen_diagram|test" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/>
</sourceEntries>
</configuration>
</storageModule>
@@ -147,7 +147,7 @@
</toolChain>
</folderInfo>
<sourceEntries>
- <entry excluding="test" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/>
+ <entry excluding="gen_diagram|test" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/>
</sourceEntries>
</configuration>
</storageModule>
diff --git a/quad/sw/modular_quad_pid/gen_diagram/Makefile b/quad/sw/modular_quad_pid/gen_diagram/Makefile
index 8b917561ca32082fa5a372a882c1e53726da8692..9c7cb2a11c2cb0bc77ee5b7b6eb0ce3bf18c3048 100644
--- a/quad/sw/modular_quad_pid/gen_diagram/Makefile
+++ b/quad/sw/modular_quad_pid/gen_diagram/Makefile
@@ -1,8 +1,8 @@
QUAD_BLOCKS = ../src/graph_blocks
-gen_diagram: generate.c
- gcc -o gen_diagram -I. -I../src/ generate.c ../src/computation_graph.c ../src/graph_blocks/*.c
+gen_diagram: generate.c ../src/control_algorithm.c ../src/computation_graph.c
+ gcc -o gen_diagram -I. -I../src/ generate.c ../src/computation_graph.c ../src/control_algorithm.c ../src/graph_blocks/*.c -Dread_flap=freadflap
.PHONY: clean
clean:
diff --git a/quad/sw/modular_quad_pid/gen_diagram/README.md b/quad/sw/modular_quad_pid/gen_diagram/README.md
index 1ab705462f7c4fcdf6a8cf2c6113c12d28bfbf41..87b45825befd9e43f373eab8b973ba098e9cd147 100644
--- a/quad/sw/modular_quad_pid/gen_diagram/README.md
+++ b/quad/sw/modular_quad_pid/gen_diagram/README.md
@@ -2,9 +2,8 @@ The gen_diagram project can create an image of the controller network. Below out
Within the `quad/sw/modular_quad_pid` directory:
-1. Copy the `control_algorithm_init` function from `src/control_algorithm.c` into `gen_diagram/generate.c`, replacing the previous implementation.
-2. Within the `gen_diagram` folder, run `make clean`. Then run `make` to build the new code.
-3. Now execute the program with `./gen_diagram`
-4. If you have graphviz installed, you can run `dot -Tpng network.dot -o network.png` to generate a PNG file of the graph.
+1. Within the `gen_diagram` folder, run `make` to build the new code.
+2. Now execute the program with `./gen_diagram`
+3. If you have graphviz installed, you can run `dot -Tpng network.dot -o network.png` to generate a PNG file of the graph.
-Enjoy your new diagram!
\ No newline at end of file
+Enjoy your new diagram!
diff --git a/quad/sw/modular_quad_pid/gen_diagram/gen_diagram b/quad/sw/modular_quad_pid/gen_diagram/gen_diagram
index 5d50c13396d1172f12984958207f5e271945d385..b350954e504a0b94686727a78ab635227a34fda8 100755
Binary files a/quad/sw/modular_quad_pid/gen_diagram/gen_diagram and b/quad/sw/modular_quad_pid/gen_diagram/gen_diagram differ
diff --git a/quad/sw/modular_quad_pid/gen_diagram/generate.c b/quad/sw/modular_quad_pid/gen_diagram/generate.c
index 3563ede331394d76ec537d1292420a19e8175d0e..d88d8598b3963b768966c16a46231e7587aa130d 100644
--- a/quad/sw/modular_quad_pid/gen_diagram/generate.c
+++ b/quad/sw/modular_quad_pid/gen_diagram/generate.c
@@ -5,49 +5,14 @@
#include "graph_blocks/node_bounds.h"
#include "graph_blocks/node_constant.h"
#include "graph_blocks/node_mixer.h"
-#include "local_PID.h"
+#include "PID.h"
+#include "control_algorithm.h"
#define ROLL_PITCH_MAX_ANGLE 0.3490 // 20 degrees
-
-typedef struct parameter_t {
- struct computation_graph* graph;
- // PID blocks
- int roll_pid;
- int pitch_pid;
- int yaw_pid;
- int roll_r_pid;
- int pitch_r_pid;
- int yaw_r_pid;
- int x_pos_pid;
- int y_pos_pid;
- int alt_pid;
- // Sensor blocks
- int cur_pitch;
- int cur_roll;
- int cur_yaw;
- int theta_dot;
- int phi_dot;
- int psi_dot;
- // RC blocks
- int rc_pitch;
- int rc_roll;
- int rc_yaw;
- int rc_throttle;
- // Loop time
- int dt;
- // Signal mixer
- int mixer;
- // Clamping blocks
- int clamp_pitch;
- int clamp_roll;
- int clamp_d_pwmR; // Clamp the change in PWM values for roll
- int clamp_d_pwmP; // ... pitch
- int clamp_d_pwmY; // ... yaw
-} parameter_t;
-
parameter_t ps;
- int control_algorithm_init(parameter_t * ps);
+//int control_algorithm_init(parameter_t * ps);
+int freadflap(int i) {return i;}
int main() {
control_algorithm_init(&ps);
@@ -56,134 +21,3 @@ int main() {
export_dot(ps.graph, dot_fp, 0);
fclose(dot_fp);
}
-
-int control_algorithm_init(parameter_t * ps)
-{
- struct computation_graph* graph = create_graph();
- ps->graph = graph;
-
- // Create all the PID blocks
- 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 bounds checking
- ps->clamp_d_pwmP = graph_add_node_bounds(graph, "P PWM Clamp");
- ps->clamp_d_pwmR = graph_add_node_bounds(graph, "R PWM Clamp");
- ps->clamp_d_pwmY = graph_add_node_bounds(graph, "Y PWM Clamp");
- ps->clamp_pitch = graph_add_node_bounds(graph, "Pitch Clamp");
- ps->clamp_roll= graph_add_node_bounds(graph, "Roll Clamp");
-
- // Create blocks for sensor inputs
- 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_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");
- 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->mixer = graph_add_node_mixer(graph, "Signal Mixer");
-
- 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_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->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_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->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_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);
- graph_set_source(graph, ps->yaw_pid, PID_CUR_POINT, ps->yaw, CONST_VAL);
- graph_set_source(graph, ps->yaw_pid, PID_DT, ps->dt, CONST_VAL);
- */
-
- // Connect angle clamping blocks
- graph_set_source(graph, ps->clamp_pitch, BOUNDS_IN, ps->y_pos_pid, PID_CORRECTION);
- graph_set_source(graph, ps->clamp_roll, BOUNDS_IN, ps->x_pos_pid, PID_CORRECTION);
-
- // Connect PWM clamping blocks
- graph_set_source(graph, ps->clamp_d_pwmP, BOUNDS_IN, ps->pitch_r_pid, PID_CORRECTION);
- graph_set_source(graph, ps->clamp_d_pwmR, BOUNDS_IN, ps->roll_r_pid, PID_CORRECTION);
- graph_set_source(graph, ps->clamp_d_pwmY, BOUNDS_IN, ps->yaw_r_pid, PID_CORRECTION);
-
- // Connect signal mixer
- graph_set_source(graph, ps->mixer, MIXER_THROTTLE, ps->rc_throttle, CONST_VAL);
- graph_set_source(graph, ps->mixer, MIXER_PITCH, ps->clamp_d_pwmP, BOUNDS_OUT);
- graph_set_source(graph, ps->mixer, MIXER_ROLL, ps->clamp_d_pwmR, BOUNDS_OUT);
- graph_set_source(graph, ps->mixer, MIXER_YAW, ps->clamp_d_pwmY, BOUNDS_OUT);
-
- // Set pitch PID constants
- graph_set_param_val(graph, ps->pitch_pid, PID_KP, PITCH_ANGLE_KP);
- graph_set_param_val(graph, ps->pitch_pid, PID_KI, PITCH_ANGLE_KI);
- graph_set_param_val(graph, ps->pitch_pid, PID_KD, PITCH_ANGLE_KD);
- graph_set_param_val(graph, ps->pitch_r_pid, PID_KP, PITCH_ANGULAR_VELOCITY_KP);
- graph_set_param_val(graph, ps->pitch_r_pid, PID_KI, PITCH_ANGULAR_VELOCITY_KI);
- graph_set_param_val(graph, ps->pitch_r_pid, PID_KD, PITCH_ANGULAR_VELOCITY_KD);
-
- // Set roll PID constants
- graph_set_param_val(graph, ps->roll_pid, PID_KP, ROLL_ANGLE_KP);
- graph_set_param_val(graph, ps->roll_pid, PID_KI, ROLL_ANGLE_KI);
- graph_set_param_val(graph, ps->roll_pid, PID_KD, ROLL_ANGLE_KD);
- graph_set_param_val(graph, ps->roll_r_pid, PID_KP, ROLL_ANGULAR_VELOCITY_KP);
- graph_set_param_val(graph, ps->roll_r_pid, PID_KI, ROLL_ANGULAR_VELOCITY_KI);
- graph_set_param_val(graph, ps->roll_r_pid, PID_KD, ROLL_ANGULAR_VELOCITY_KD);
-
- // Set yaw PID constants
- graph_set_param_val(graph, ps->yaw_pid, PID_KP, YAW_ANGLE_KP);
- graph_set_param_val(graph, ps->yaw_pid, PID_KI, YAW_ANGLE_KI);
- graph_set_param_val(graph, ps->yaw_pid, PID_KD, YAW_ANGLE_KD);
- graph_set_param_val(graph, ps->yaw_r_pid, PID_KP, YAW_ANGULAR_VELOCITY_KP);
- graph_set_param_val(graph, ps->yaw_r_pid, PID_KI, YAW_ANGULAR_VELOCITY_KI);
- graph_set_param_val(graph, ps->yaw_r_pid, PID_KD, YAW_ANGULAR_VELOCITY_KD);
-
- // Set angle clamping limits
- graph_set_param_val(graph, ps->clamp_pitch, BOUNDS_MIN, -ROLL_PITCH_MAX_ANGLE);
- graph_set_param_val(graph, ps->clamp_pitch, BOUNDS_MAX, ROLL_PITCH_MAX_ANGLE);
- graph_set_param_val(graph, ps->clamp_roll, BOUNDS_MIN, -ROLL_PITCH_MAX_ANGLE);
- graph_set_param_val(graph, ps->clamp_roll, BOUNDS_MAX, ROLL_PITCH_MAX_ANGLE);
-
- // Set PWM difference clamping limits
- graph_set_param_val(graph, ps->clamp_d_pwmP, BOUNDS_MIN, -20000);
- graph_set_param_val(graph, ps->clamp_d_pwmP, BOUNDS_MAX, 20000);
- graph_set_param_val(graph, ps->clamp_d_pwmR, BOUNDS_MIN, -20000);
- graph_set_param_val(graph, ps->clamp_d_pwmR, BOUNDS_MAX, 20000);
- graph_set_param_val(graph, ps->clamp_d_pwmY, BOUNDS_MIN, -20000);
- graph_set_param_val(graph, ps->clamp_d_pwmY, BOUNDS_MAX, 20000);
-
- // TODO: Change this to use LOOP_TIME
- graph_set_param_val(graph, ps->dt, CONST_SET, 0.005);
-
- return 0;
-}
diff --git a/quad/sw/modular_quad_pid/gen_diagram/network.dot b/quad/sw/modular_quad_pid/gen_diagram/network.dot
index 7e628deb20a80511b4d5f0c6529d3336bfa9bf21..7e8830be9be82d84457304100e8043d469b9d387 100644
--- a/quad/sw/modular_quad_pid/gen_diagram/network.dot
+++ b/quad/sw/modular_quad_pid/gen_diagram/network.dot
@@ -10,9 +10,9 @@ label="
|<f5> [Ki=0.000]
|<f6> [Kd=0.200]
"]
-"Roll" -> "Roll PID":f1
-"RC Roll" -> "Roll PID":f2
-"dT" -> "Roll PID":f3
+"Roll" -> "Roll PID":f1 [label="Constant"]
+"RC Roll" -> "Roll PID":f2 [label="Constant"]
+"dT" -> "Roll PID":f3 [label="Constant"]
"Pitch PID"[shape=record
label="
<f0> Pitch PID
@@ -23,9 +23,9 @@ label="
|<f5> [Ki=0.000]
|<f6> [Kd=0.200]
"]
-"Pitch" -> "Pitch PID":f1
-"RC Pitch" -> "Pitch PID":f2
-"dT" -> "Pitch PID":f3
+"Pitch" -> "Pitch PID":f1 [label="Constant"]
+"RC Pitch" -> "Pitch PID":f2 [label="Constant"]
+"dT" -> "Pitch PID":f3 [label="Constant"]
"Yaw PID"[shape=record
label="
<f0> Yaw PID
@@ -46,9 +46,9 @@ label="
|<f5> [Ki=0.000]
|<f6> [Kd=550.000]
"]
-"dPhi" -> "Roll Rate PID":f1
-"Roll PID" -> "Roll Rate PID":f2
-"dT" -> "Roll Rate PID":f3
+"dPhi" -> "Roll Rate PID":f1 [label="Constant"]
+"Roll PID" -> "Roll Rate PID":f2 [label="Correction"]
+"dT" -> "Roll Rate PID":f3 [label="Constant"]
"Pitch Rate PID"[shape=record
label="
<f0> Pitch Rate PID
@@ -59,9 +59,9 @@ label="
|<f5> [Ki=0.000]
|<f6> [Kd=550.000]
"]
-"dTheta" -> "Pitch Rate PID":f1
-"Pitch PID" -> "Pitch Rate PID":f2
-"dT" -> "Pitch Rate PID":f3
+"dTheta" -> "Pitch Rate PID":f1 [label="Constant"]
+"Pitch PID" -> "Pitch Rate PID":f2 [label="Correction"]
+"dT" -> "Pitch Rate PID":f3 [label="Constant"]
"Yaw Rate PID"[shape=record
label="
<f0> Yaw Rate PID
@@ -72,12 +72,12 @@ label="
|<f5> [Ki=0.000]
|<f6> [Kd=0.000]
"]
-"dPsi" -> "Yaw Rate PID":f1
-"RC Yaw" -> "Yaw Rate PID":f2
-"dT" -> "Yaw Rate PID":f3
-"X pos PID PID"[shape=record
+"dPsi" -> "Yaw Rate PID":f1 [label="Constant"]
+"RC Yaw" -> "Yaw Rate PID":f2 [label="Constant"]
+"dT" -> "Yaw Rate PID":f3 [label="Constant"]
+"X pos PID"[shape=record
label="
-<f0> X pos PID PID
+<f0> X pos PID
|<f1> --\>Cur point
|<f2> --\>Setpoint
|<f3> --\>dt
@@ -112,7 +112,7 @@ label="
|<f2> [Min=-20000.000]
|<f3> [Max=20000.000]
"]
-"Pitch Rate PID" -> "P PWM Clamp":f1
+"Pitch Rate PID" -> "P PWM Clamp":f1 [label="Correction"]
"R PWM Clamp"[shape=record
label="
<f0> R PWM Clamp
@@ -120,7 +120,7 @@ label="
|<f2> [Min=-20000.000]
|<f3> [Max=20000.000]
"]
-"Roll Rate PID" -> "R PWM Clamp":f1
+"Roll Rate PID" -> "R PWM Clamp":f1 [label="Correction"]
"Y PWM Clamp"[shape=record
label="
<f0> Y PWM Clamp
@@ -128,7 +128,7 @@ label="
|<f2> [Min=-20000.000]
|<f3> [Max=20000.000]
"]
-"Yaw Rate PID" -> "Y PWM Clamp":f1
+"Yaw Rate PID" -> "Y PWM Clamp":f1 [label="Correction"]
"Pitch Clamp"[shape=record
label="
<f0> Pitch Clamp
@@ -136,7 +136,7 @@ label="
|<f2> [Min=-0.349]
|<f3> [Max=0.349]
"]
-"Y pos PID" -> "Pitch Clamp":f1
+"Y pos PID" -> "Pitch Clamp":f1 [label="Correction"]
"Roll Clamp"[shape=record
label="
<f0> Roll Clamp
@@ -144,7 +144,7 @@ label="
|<f2> [Min=-0.349]
|<f3> [Max=0.349]
"]
-"X pos PID PID" -> "Roll Clamp":f1
+"X pos PID" -> "Roll Clamp":f1 [label="Correction"]
"Pitch"[shape=record
label="
<f0> Pitch
@@ -203,10 +203,10 @@ label="
|<f3> --\>Roll
|<f4> --\>Yaw
"]
-"RC Throttle" -> "Signal Mixer":f1
-"P PWM Clamp" -> "Signal Mixer":f2
-"R PWM Clamp" -> "Signal Mixer":f3
-"Y PWM Clamp" -> "Signal Mixer":f4
+"RC Throttle" -> "Signal Mixer":f1 [label="Constant"]
+"P PWM Clamp" -> "Signal Mixer":f2 [label="Bounded"]
+"R PWM Clamp" -> "Signal Mixer":f3 [label="Bounded"]
+"Y PWM Clamp" -> "Signal Mixer":f4 [label="Bounded"]
"dT"[shape=record
label="
<f0> dT
diff --git a/quad/sw/modular_quad_pid/src/PID.h b/quad/sw/modular_quad_pid/src/PID.h
index d0e69ddcc9450d9888b8e7a424e45aabb46b2039..a7701ab2a8a8701343d4e65d5688759d70aba2b9 100644
--- a/quad/sw/modular_quad_pid/src/PID.h
+++ b/quad/sw/modular_quad_pid/src/PID.h
@@ -81,7 +81,4 @@
#define ALT_ZPOS_KI 817.0f
#define ALT_ZPOS_KD 7353.0f
-// Computes control error and correction
-PID_values pid_computation(PID_t *pid);
-
#endif /* PID_H_ */
diff --git a/quad/sw/modular_quad_pid/src/actuator_command_processing.c b/quad/sw/modular_quad_pid/src/actuator_command_processing.c
index 64fce135a7eb95bb5a80962e72d64572dbccf574..e27dbf434d1aba16341a67fc3115f3ea2d88725f 100644
--- a/quad/sw/modular_quad_pid/src/actuator_command_processing.c
+++ b/quad/sw/modular_quad_pid/src/actuator_command_processing.c
@@ -7,6 +7,7 @@
#include "actuator_command_processing.h"
#include "sensor_processing.h"
+#include "controllers.h"
int actuator_command_processing(log_t* log_struct, user_input_t * user_input_struct, raw_actuator_t* raw_actuator_struct, actuator_command_t* actuator_command_struct)
{
diff --git a/quad/sw/modular_quad_pid/src/control_algorithm.c b/quad/sw/modular_quad_pid/src/control_algorithm.c
index d9d216b73825916b424044c4360f00fcea0bcceb..3623d4d6f92a3c0c2e67af047d9f919fff9e4165 100644
--- a/quad/sw/modular_quad_pid/src/control_algorithm.c
+++ b/quad/sw/modular_quad_pid/src/control_algorithm.c
@@ -8,11 +8,12 @@
// This implemented modular quadrotor software implements a PID control algorithm
#include "control_algorithm.h"
-#include "communication.h"
#include "graph_blocks/node_pid.h"
#include "graph_blocks/node_bounds.h"
#include "graph_blocks/node_constant.h"
#include "graph_blocks/node_mixer.h"
+#include "PID.h"
+#include "util.h"
#define ROLL_PITCH_MAX_ANGLE 0.3490 // 20 degrees
@@ -28,7 +29,7 @@
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->x_pos_pid = graph_add_node_pid(graph, "X pos PID");
ps->y_pos_pid = graph_add_node_pid(graph, "Y pos PID");
ps->alt_pid = graph_add_node_pid(graph, "Altitude");
diff --git a/quad/sw/modular_quad_pid/src/initialize_components.c b/quad/sw/modular_quad_pid/src/initialize_components.c
index c548d26d2151f6923fc91421211a2f938f3e9a4b..0009bac9b4a7d4de994376f8b837b4ea1e957cd9 100644
--- a/quad/sw/modular_quad_pid/src/initialize_components.c
+++ b/quad/sw/modular_quad_pid/src/initialize_components.c
@@ -7,6 +7,7 @@
#include "initialize_components.h"
#include "communication.h"
+#include "sensor.h"
//#define BENCH_TEST
diff --git a/quad/sw/modular_quad_pid/src/log_data.c b/quad/sw/modular_quad_pid/src/log_data.c
index 2ab248cdda53c460527bf294e95a0091e5a47151..8a91a40be2d650c8685ebc256dfb88cbb67bbba2 100644
--- a/quad/sw/modular_quad_pid/src/log_data.c
+++ b/quad/sw/modular_quad_pid/src/log_data.c
@@ -5,6 +5,13 @@
* Author: ucart
*/
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include "PID.h"
+#include "type_def.h"
+#include "uart.h"
+#include "sleep.h"
#include "log_data.h"
#include "communication.h"
diff --git a/quad/sw/modular_quad_pid/src/log_data.h b/quad/sw/modular_quad_pid/src/log_data.h
index 994d043ba18abd76c73e8e95ba7e93f514c941e2..14d06b3bb9b389d52b29e67f93d04201a8f33220 100644
--- a/quad/sw/modular_quad_pid/src/log_data.h
+++ b/quad/sw/modular_quad_pid/src/log_data.h
@@ -7,14 +7,7 @@
#ifndef LOG_DATA_H_
#define LOG_DATA_H_
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include "PID.h"
#include "type_def.h"
-#include "uart.h"
-#include "sleep.h"
#define LOG_STARTING_SIZE 4096 //262144 // 2^18 32768 2^15
diff --git a/quad/sw/modular_quad_pid/src/send_actuator_commands.c b/quad/sw/modular_quad_pid/src/send_actuator_commands.c
index 7174348923252cf7109fd1d2df8d08bcef673313..85b1ca76b6df58a472f68cdd5df65a1937932674 100644
--- a/quad/sw/modular_quad_pid/src/send_actuator_commands.c
+++ b/quad/sw/modular_quad_pid/src/send_actuator_commands.c
@@ -6,6 +6,7 @@
*/
#include "send_actuator_commands.h"
+#include "util.h"
int send_actuator_commands(log_t* log_struct, actuator_command_t* actuator_command_struct)
{
diff --git a/quad/sw/modular_quad_pid/src/sensor_processing.c b/quad/sw/modular_quad_pid/src/sensor_processing.c
index 5fd682c3fae6a0bf0039932e24a20dc6eff27120..9465b0228a0f5e36be563d4fcf95ce2d6670701e 100644
--- a/quad/sw/modular_quad_pid/src/sensor_processing.c
+++ b/quad/sw/modular_quad_pid/src/sensor_processing.c
@@ -5,6 +5,11 @@
* Author: ucart
*/
+#include <stdio.h>
+#include "log_data.h"
+#include "sensor.h"
+#include "conversion.h"
+#include "quadposition.h"
#include "sensor_processing.h"
#include "timer.h"
#include <math.h>
diff --git a/quad/sw/modular_quad_pid/src/sensor_processing.h b/quad/sw/modular_quad_pid/src/sensor_processing.h
index cf3fd122b33168782cccbb7dd1edc012ca3c2107..04b5efebe48ab20bdecfcfaab11e1ffe2f63a540 100644
--- a/quad/sw/modular_quad_pid/src/sensor_processing.h
+++ b/quad/sw/modular_quad_pid/src/sensor_processing.h
@@ -7,12 +7,7 @@
#ifndef SENSOR_PROCESSING_H_
#define SENSOR_PROCESSING_H_
-
-#include <stdio.h>
-#include "log_data.h"
-#include "sensor.h"
-#include "conversion.h"
-#include "quadposition.h"
+#include "type_def.h"
/*
* Aero channel declaration
*/
diff --git a/quad/sw/modular_quad_pid/src/user_input.c b/quad/sw/modular_quad_pid/src/user_input.c
index bc0c4c5b7047afa4cdecf6093e48a6b977bf9a15..91fb830acf07a8b1d1eaaf44175554625d0cd233 100644
--- a/quad/sw/modular_quad_pid/src/user_input.c
+++ b/quad/sw/modular_quad_pid/src/user_input.c
@@ -7,6 +7,7 @@
#include "user_input.h"
#include "uart.h"
+#include "controllers.h"
int get_user_input(log_t* log_struct, user_input_t* user_input_struct)
{
diff --git a/quad/sw/modular_quad_pid/src/util.c b/quad/sw/modular_quad_pid/src/util.c
index c7dd33a1ca988a454de416311fca18fcfbc5fe1c..be07d38187f621dc63405b031eb5ff3131d81a5b 100644
--- a/quad/sw/modular_quad_pid/src/util.c
+++ b/quad/sw/modular_quad_pid/src/util.c
@@ -7,6 +7,18 @@
#include "util.h"
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <math.h>
+#include <xgpiops.h>
+#include "PID.h"
+#include "log_data.h"
+#include <sleep.h>
+#include "controllers.h"
+#include "xparameters.h"
+#include "uart.h"
+
extern int motor0_bias, motor1_bias, motor2_bias, motor3_bias;
//Global variable representing the current pulseW
int pulseW = pulse_throttle_low;
diff --git a/quad/sw/modular_quad_pid/src/util.h b/quad/sw/modular_quad_pid/src/util.h
index 285db6085f1804ebca654ec969958adf3abd9955..7c864f4d6ef165749649293280f4630805b89045 100644
--- a/quad/sw/modular_quad_pid/src/util.h
+++ b/quad/sw/modular_quad_pid/src/util.h
@@ -7,17 +7,7 @@
#ifndef _UTIL_H
#define _UTIL_H
-#include <stdlib.h>
-#include <stdio.h>
-#include <string.h>
-#include <math.h>
-#include <xgpiops.h>
-#include "PID.h"
-#include "log_data.h"
-#include <sleep.h>
-#include "controllers.h"
-#include "xparameters.h"
-#include "uart.h"
+#include "type_def.h"
#define clock_rate 100000000