diff --git a/controls/DataAnalysisTool/Tool/simplePlots.m b/controls/DataAnalysisTool/Tool/simplePlots.m
new file mode 100644
index 0000000000000000000000000000000000000000..edd91bf4aa05002531109897c7c0f6ca162ed387
--- /dev/null
+++ b/controls/DataAnalysisTool/Tool/simplePlots.m
@@ -0,0 +1,70 @@
+%%
+plot(Time, Cam_Meas_Roll*(180/pi)); hold on;
+plot(Time, Quad_Meas_Roll*(180/pi));
+
+%%
+plot(Time, pitch_velocity*(180/pi)); hold on;
+plot(Time, roll_velocity*(180/pi));
+plot(Time, yaw_velocity*(180/pi));
+
+%%
+figure;
+plot(time, VRPNPitchConstant*(180/pi)); hold on;
+plot(time, PitchConstant*(180/pi));
+legend('Camera Pitch', 'Quad Pitch');
+xlabel('seconds'); ylabel('degrees');
+
+%%
+figure;
+plot(time, (VRPNRollConstant + pi)*(180/pi)); hold on;
+plot(time, RollConstant*(180/pi));
+legend('Camera Roll', 'Quad Roll');
+xlabel('seconds'); ylabel('degrees');
+
+%%
+%plot(Time, X_setpoint); hold on;
+markerSize = 3;
+ax1 = subplot(2,1,1);
+plot(time, (XSetpointConstant - VRPNXConstant), '-o', 'MarkerSize', markerSize); hold on;
+plot(time, -XposPIDCorrection * (180/pi), '-o', 'MarkerSize', markerSize); hold off;
+legend('X Error', 'X PID output');
+
+
+ax2 = subplot(2,1,2);
+plot(time, (180/pi)*PitchConstant, '-o', 'MarkerSize', markerSize); hold on;
+plot(time, (180/pi)*PitchPIDCorrection, '-o', 'MarkerSize', markerSize);
+legend('Pitch Error', 'Pitch PID output');
+
+linkaxes([ax1, ax2], 'x');
+%%
+ax2 = subplot(2,2,1);
+plot(time, XSetpointConstant - VRPNXConstant);
+title('X error');
+
+ax1 = subplot(2,2,2);
+plot(time, XposPIDCorrection);
+title('x output');
+
+ax3 = subplot(2,2,3);
+plot(time, PitchPIDCorrection); hold on;
+plot(time, VRPNPitchConstant .* 10);
+title('pitch output');
+legend('output', 'Pitch');
+
+ax4 = subplot(2,2,4);
+plot(time, PitchRatePIDCorrection); hold on;
+plot(time, gyro_y .* 1044.26);
+legend('output', 'Pitch rate');
+title('pitch rate output');
+
+linkaxes([ax1, ax2, ax3, ax4], 'x');
+%%
+plot(time, 1044.26 .* (PitchPIDCorrection - gyro_y));hold on;
+%plot(time, PitchRatePIDCorrection);
+%%
+ax2 = subplot(2, 1, 1);
+plot(time, YawConstant);
+ax1 = subplot(2, 1, 2);
+plot(time, gyro_z); hold on;
+plot(time, YawRatePIDCorrection);
+linkaxes([ax1, ax2], 'x');
\ No newline at end of file
diff --git a/controls/model/c_controller.c b/controls/model/c_controller.c
new file mode 100644
index 0000000000000000000000000000000000000000..f9a5ac36e34658468615f33afb67131209858694
--- /dev/null
+++ b/controls/model/c_controller.c
@@ -0,0 +1,71 @@
+#include "c_controller.h"
+
+double c_controller(int vrpn_id, double vrpn_ts, double set_x, double set_y, double set_z, double set_yaw,
+ double cur_x, double cur_y, double cur_z,
+ double cur_phi, double cur_theta, double cur_psi,
+ double cur_phi_d, double cur_theta_d, double cur_psi_d,
+ double *z_out, double *y_out, double *x_out, double *yaw_out,
+ double* pid_y_out, double* pid_roll_out) {
+
+ static log_t log_struct;
+ static user_input_t user_input_struct;
+ static sensor_t sensor_struct;
+ static setpoint_t setpoint_struct;
+ static parameter_t ps;
+ static user_defined_t user_defined_struct;
+ static actuator_command_t actuator_struct;
+ static modular_structs_t structs;
+
+ static int last_vrpn_id = -1;
+ static int initialized = 0;
+ if (!initialized) {
+ control_algorithm_init(&ps);
+ sensor_struct.currentQuadPosition.packetId = vrpn_id;
+ initialized = 1;
+ }
+
+ // Check VRPN update
+ if (vrpn_id != last_vrpn_id) {
+ user_input_struct.locationFresh = 1;
+ int vrpn_ts_packets = (vrpn_ts / 0.01) + 0.5; // + 0.5 to round
+ sensor_struct.currentQuadPosition.packetId += vrpn_ts_packets;
+ // VRPN values
+ sensor_struct.currentQuadPosition.x_pos = cur_x;
+ sensor_struct.currentQuadPosition.y_pos = cur_y;
+ sensor_struct.currentQuadPosition.alt_pos = cur_z;
+ sensor_struct.currentQuadPosition.pitch = cur_theta;
+ sensor_struct.currentQuadPosition.roll = cur_phi;
+ sensor_struct.currentQuadPosition.yaw = cur_psi;
+ last_vrpn_id = vrpn_id;
+ }
+
+ // Setpoints
+ /*
+ setpoint_struct.desiredQuadPosition.x_pos = set_x;
+ setpoint_struct.desiredQuadPosition.y_pos = set_y;
+ setpoint_struct.desiredQuadPosition.alt_pos = set_z;
+ setpoint_struct.desiredQuadPosition.yaw = set_yaw;
+ */
+ int i;
+
+ // Sensors
+ sensor_struct.pitch_angle_filtered = cur_theta;
+ sensor_struct.roll_angle_filtered = cur_phi;
+ sensor_struct.theta_dot = cur_theta_d;
+ sensor_struct.phi_dot = cur_phi_d;
+ sensor_struct.psi_dot = cur_psi_d;
+
+ control_algorithm(&log_struct, &user_input_struct, &sensor_struct, &setpoint_struct, &ps, &user_defined_struct, &actuator_struct, NULL);
+
+ *z_out = graph_get_output(ps.graph, ps.alt_pid, PID_CORRECTION);
+ *y_out = graph_get_output(ps.graph, ps.roll_r_pid, PID_CORRECTION);
+ *x_out = graph_get_output(ps.graph, ps.pitch_r_pid, PID_CORRECTION);
+ *yaw_out = graph_get_output(ps.graph, ps.yaw_r_pid, PID_CORRECTION);
+
+ *pid_y_out = graph_get_output(ps.graph, ps.y_pos_pid, PID_CORRECTION);
+ *pid_roll_out = graph_get_output(ps.graph, ps.roll_pid, PID_CORRECTION);
+ return 0;
+}
+
+int read_flap(int flap) { return AUTO_FLIGHT_MODE; }
+float get_last_loop_time() { return 0.005; }
diff --git a/controls/model/c_controller.h b/controls/model/c_controller.h
new file mode 100644
index 0000000000000000000000000000000000000000..e53b1a508409d70211a99fe6ba6de76e092ae7a5
--- /dev/null
+++ b/controls/model/c_controller.h
@@ -0,0 +1,18 @@
+#ifndef C_CONTROLLER_H
+#define C_CONTROLLER_H
+#include "quad_files/computation_graph.h"
+#include "quad_files/graph_blocks/node_pid.h"
+#include "quad_files/graph_blocks/node_bounds.h"
+#include "quad_files/graph_blocks/node_constant.h"
+#include "quad_files/graph_blocks/node_mixer.h"
+#include "quad_files/PID.h"
+#include "quad_files/control_algorithm.h"
+
+double c_controller(int vrpn_id, double vrpn_ts, double set_x, double set_y, double set_z, double set_yaw,
+ double cur_x, double cur_y, double cur_z,
+ double cur_phi, double cur_theta, double cur_psi,
+ double cur_phi_d, double cur_theta_d, double cur_psi_d,
+ double *z_out, double *y_out, double *x_out, double *yaw_out,
+ double* pid_y_out, double* pid_roll_out);
+
+#endif // C_CONTROLLER_H
\ No newline at end of file
diff --git a/controls/model/modelParameters.m b/controls/model/modelParameters.m
index e457e0332bd2705446dba5aa74145ebb9f26aeb0..f01946f1f512a9e114c26b68b801b0581d243f04 100644
--- a/controls/model/modelParameters.m
+++ b/controls/model/modelParameters.m
@@ -20,7 +20,8 @@
If = 0.511; % Motor internal friction current
Pmin = 1e5; % Minimum zybo output duty cycle command
Pmax = 2e5; % Maximum zybo output duty cycle command
- Tc = 0.01; % Camera system sampling period
+ Tc = 0.04; % Camera system sampling period
+ Tq = 0.005; % Quad sampling period
tau_c = 0; % Camera system total latency
Vb = 11.1; % Nominal battery voltage (V)
x_controlled_o = 0; % Equilibrium lateral controller output
diff --git a/controls/model/quad_files/.gitignore b/controls/model/quad_files/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..1c4bc674e912af84e9c6bed639cd5780a68b7846
--- /dev/null
+++ b/controls/model/quad_files/.gitignore
@@ -0,0 +1,3 @@
+# Don't track the copied files here
+*.h
+*.c
\ No newline at end of file
diff --git a/controls/model/quad_files/copy_files.bat b/controls/model/quad_files/copy_files.bat
new file mode 100644
index 0000000000000000000000000000000000000000..bd5f65861b103ddaf3b761a47c9bc584bb35b50b
--- /dev/null
+++ b/controls/model/quad_files/copy_files.bat
@@ -0,0 +1,27 @@
+copy ..\..\..\quad\computation_graph\src\computation_graph.c computation_graph.c
+copy ..\..\..\quad\computation_graph\src\computation_graph.h computation_graph.h
+
+copy ..\..\..\quad\sw\modular_quad_pid\src\control_algorithm.h control_algorithm.h
+copy ..\..\..\quad\sw\modular_quad_pid\src\control_algorithm.c control_algorithm.c
+
+copy ..\..\..\quad\sw\modular_quad_pid\src\PID.h PID.h
+copy ..\..\..\quad\sw\modular_quad_pid\src\type_def.h type_def.h
+copy ..\..\..\quad\sw\modular_quad_pid\src\sensor_processing.h sensor_processing.h
+copy ..\..\..\quad\sw\modular_quad_pid\src\log_data.h log_data.h
+copy ..\..\..\quad\sw\modular_quad_pid\src\quadposition.h quadposition.h
+copy ..\..\..\quad\sw\modular_quad_pid\src\util.h util.h
+copy ..\..\..\quad\sw\modular_quad_pid\src\timer.h timer.h
+copy ..\..\..\groundStation\src\backend\commands.h commands.h
+copy ..\..\..\groundStation\src\backend\callbacks.h callbacks.h
+
+mkdir graph_blocks
+copy ..\..\..\quad\computation_graph\src\graph_blocks\node_constant.h graph_blocks\node_constant.h
+copy ..\..\..\quad\computation_graph\src\graph_blocks\node_constant.c graph_blocks\node_constant.c
+copy ..\..\..\quad\computation_graph\src\graph_blocks\node_add.h graph_blocks\node_add.h
+copy ..\..\..\quad\computation_graph\src\graph_blocks\node_add.c graph_blocks\node_add.c
+copy ..\..\..\quad\sw\modular_quad_pid\src\graph_blocks\node_mixer.c graph_blocks\node_mixer.c
+copy ..\..\..\quad\sw\modular_quad_pid\src\graph_blocks\node_mixer.h graph_blocks\node_mixer.h
+copy ..\..\..\quad\sw\modular_quad_pid\src\graph_blocks\node_pid.c graph_blocks\node_pid.c
+copy ..\..\..\quad\sw\modular_quad_pid\src\graph_blocks\node_pid.h graph_blocks\node_pid.h
+copy ..\..\..\quad\sw\modular_quad_pid\src\graph_blocks\node_bounds.c graph_blocks\node_bounds.c
+copy ..\..\..\quad\sw\modular_quad_pid\src\graph_blocks\node_bounds.h graph_blocks\node_bounds.h
\ No newline at end of file
diff --git a/controls/model/test_model.slx b/controls/model/test_model.slx
index 805e43a9e1d83040c87b713476195126ca17d371..fc7200310cdc5010d64c1a4a41cdf9d075705d1f 100644
Binary files a/controls/model/test_model.slx and b/controls/model/test_model.slx differ
diff --git a/quad/computation_graph/src/graph_blocks/node_add.c b/quad/computation_graph/src/graph_blocks/node_add.c
index eefb22fde9ae9a5cc72d2eb679905f2e70038ac9..048a49b3b6a9c054f74900e356e6377f17a6a4b7 100644
--- a/quad/computation_graph/src/graph_blocks/node_add.c
+++ b/quad/computation_graph/src/graph_blocks/node_add.c
@@ -8,7 +8,7 @@ static void reset(void *state) {}
static const char* const in_names[2] = {"Summand 1", "Summand 2"};
static const char* const out_names[1] = {"Sum"};
-static const char* const param_names[0] = {};
+static const char* const param_names[1] = {"Error if you see this"};
const struct graph_node_type node_add_type = {
.input_names = in_names,
.output_names = out_names,
diff --git a/quad/computation_graph/src/graph_blocks/node_constant.c b/quad/computation_graph/src/graph_blocks/node_constant.c
index 8ad9f8c5afdb5dc8f4cfacac00f2725ad388a59a..b2a46e757eef397b4c9805d4b76a159fccc6c9a0 100644
--- a/quad/computation_graph/src/graph_blocks/node_constant.c
+++ b/quad/computation_graph/src/graph_blocks/node_constant.c
@@ -6,7 +6,7 @@ static void output_const(void *state, const double *params, const double *inputs
}
static void reset(void *state) {}
-static const char* const in_names[0] = {};
+static const char* const in_names[1] = {"You shouldn't see this"};
static const char* const out_names[1] = {"Constant"};
static const char* const param_names[1] = {"Constant"};
const struct graph_node_type node_const_type = {
diff --git a/quad/sw/modular_quad_pid/src/graph_blocks/node_mixer.c b/quad/sw/modular_quad_pid/src/graph_blocks/node_mixer.c
index 8b702b360654b3e957d19743abc24b433f58f64d..316c1ce8218b835ee002c8c4352c97af111b318d 100644
--- a/quad/sw/modular_quad_pid/src/graph_blocks/node_mixer.c
+++ b/quad/sw/modular_quad_pid/src/graph_blocks/node_mixer.c
@@ -25,7 +25,7 @@ static void reset_mixer(void *state) {}
static const char* const in_names[4] = {"Throttle", "Pitch", "Roll", "Yaw"};
static const char* const out_names[4] = {"PWM 0", "PWM 1", "PWM 2", "PWM 3"};
-static const char* const param_names[0] = {};
+static const char* const param_names[1] = {"You shouldnt see this"};
const struct graph_node_type node_mixer_type = {
.input_names = in_names,
.output_names = out_names,