diff --git a/quad/sw/modular_quad_pid/gen_diagram/Makefile b/quad/sw/modular_quad_pid/gen_diagram/Makefile
new file mode 100644
index 0000000000000000000000000000000000000000..610e289a0430678f42d41648f6c8f98143450b6a
--- /dev/null
+++ b/quad/sw/modular_quad_pid/gen_diagram/Makefile
@@ -0,0 +1,10 @@
+
+COMP_GRAPH = ../../../computation_graph
+QUAD_BLOCKS = ../src/graph_blocks
+
+gen_diagram: generate.c
+ gcc -o gen_diagram -I. -I$(COMP_GRAPH)/src generate.c $(COMP_GRAPH)/src/computation_graph.c $(QUAD_BLOCKS)/node_bounds.c $(QUAD_BLOCKS)/node_pid.c $(QUAD_BLOCKS)/node_mixer.c
+
+.PHONY: clean
+clean:
+ rm gen_diagram
diff --git a/quad/sw/modular_quad_pid/gen_diagram/gen_diagram b/quad/sw/modular_quad_pid/gen_diagram/gen_diagram
new file mode 100644
index 0000000000000000000000000000000000000000..ae1620225638cc42929390bc1646225a8b1cbbf3
Binary files /dev/null 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
new file mode 100644
index 0000000000000000000000000000000000000000..b7cc7966c71e37df5759f62aa0c6c6a938b94358
--- /dev/null
+++ b/quad/sw/modular_quad_pid/gen_diagram/generate.c
@@ -0,0 +1,185 @@
+#include <stdio.h>
+#include "computation_graph.h"
+#include "../src/graph_blocks/node_pid.h"
+#include "../src/graph_blocks/node_bounds.h"
+#include "graph_blocks/node_constant.h"
+#include "../src/graph_blocks/node_mixer.h"
+#include "local_PID.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 main() {
+
+ FILE* dot_fp;
+ dot_fp = fopen(".\\network.dot", "w");
+ export_dot(ps.graph, dot_fp);
+ 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 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/local_PID.h b/quad/sw/modular_quad_pid/gen_diagram/local_PID.h
new file mode 100644
index 0000000000000000000000000000000000000000..376b1f1b921d8aceab6da3120aa58abccdb8a103
--- /dev/null
+++ b/quad/sw/modular_quad_pid/gen_diagram/local_PID.h
@@ -0,0 +1,85 @@
+/*
+ * PID.h
+ *
+ * Created on: Nov 10, 2014
+ * Author: ucart
+ */
+
+#ifndef PID_H_
+#define PID_H_
+
+// Yaw constants
+
+// when using units of degrees
+//#define YAW_ANGULAR_VELOCITY_KP 40.0f
+//#define YAW_ANGULAR_VELOCITY_KI 0.0f
+//#define YAW_ANGULAR_VELOCITY_KD 0.0f
+//#define YAW_ANGLE_KP 2.6f
+//#define YAW_ANGLE_KI 0.0f
+//#define YAW_ANGLE_KD 0.0f
+
+// when using units of radians
+#define YAW_ANGULAR_VELOCITY_KP 190.0f * 2292.0f//200.0f * 2292.0f
+#define YAW_ANGULAR_VELOCITY_KI 0.0f
+#define YAW_ANGULAR_VELOCITY_KD 0.0f
+#define YAW_ANGLE_KP 2.6f
+#define YAW_ANGLE_KI 0.0f
+#define YAW_ANGLE_KD 0.0f
+
+// Roll constants
+//#define ROLL_ANGULAR_VELOCITY_KP 0.95f
+//#define ROLL_ANGULAR_VELOCITY_KI 0.0f
+//#define ROLL_ANGULAR_VELOCITY_KD 0.13f//0.4f//0.7f
+//#define ROLL_ANGLE_KP 17.0f //9.0f
+//#define ROLL_ANGLE_KI 0.0f
+//#define ROLL_ANGLE_KD 0.3f // 0.2f
+//#define YPOS_KP 0.0f
+//#define YPOS_KI 0.0f
+//#define YPOS_KD 0.0f
+
+// when using units of radians
+#define ROLL_ANGULAR_VELOCITY_KP 100.0f*46.0f//102.0f*46.0f//9384.0f//204.0f * 46.0f
+#define ROLL_ANGULAR_VELOCITY_KI 0.0f
+#define ROLL_ANGULAR_VELOCITY_KD 100.f*5.5f//102.0f*6.8f//1387.2//204.0f * 6.8f
+#define ROLL_ANGLE_KP 15.0f
+#define ROLL_ANGLE_KI 0.0f
+#define ROLL_ANGLE_KD 0.2f
+#define YPOS_KP 0.015f
+#define YPOS_KI 0.005f
+#define YPOS_KD 0.03f
+
+
+//Pitch constants
+
+// when using units of degrees
+//#define PITCH_ANGULAR_VELOCITY_KP 0.95f
+//#define PITCH_ANGULAR_VELOCITY_KI 0.0f
+//#define PITCH_ANGULAR_VELOCITY_KD 0.13f//0.35f//0.7f
+//#define PITCH_ANGLE_KP 17.0f // 7.2f
+//#define PITCH_ANGLE_KI 0.0f
+//#define PITCH_ANGLE_KD 0.3f //0.3f
+//#define XPOS_KP 40.0f
+//#define XPOS_KI 0.0f
+//#define XPOS_KD 10.0f//0.015f
+
+// when using units of radians
+#define PITCH_ANGULAR_VELOCITY_KP 100.0f*46.0f//101.0f*46.0f//9292.0f//202.0f * 46.0f
+#define PITCH_ANGULAR_VELOCITY_KI 0.0f
+#define PITCH_ANGULAR_VELOCITY_KD 100.0f*5.5f//101.0f*6.8f//1373.6//202.0f * 6.8f
+#define PITCH_ANGLE_KP 15.0f
+#define PITCH_ANGLE_KI 0.0f
+#define PITCH_ANGLE_KD 0.2f
+#define XPOS_KP -0.015f
+#define XPOS_KI -0.005f
+#define XPOS_KD -0.03f
+
+
+//Throttle constants
+#define ALT_ZPOS_KP 9804.0f
+#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_ */