Added quad control_algorithm to Simulink model.

controls/DataAnalysisTool/Tool/simplePlots.m

+%% 
+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

controls/model/c_controller.c

+#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; }

controls/model/c_controller.h

+#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

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

controls/model/quad_files/.gitignore

+# Don't track the copied files here
+*.h
+*.c
\ No newline at end of file

controls/model/quad_files/copy_files.bat

+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

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,

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 = {

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,