Merge branch 'master' of git.ece.iastate.edu:danc/MicroCART_17-18

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"
+#include "quad_files/control_algorithm.c"
+#include "quad_files/computation_graph.c"
+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/computation_graph.c

@@ -103,7 +103,7 @@ int graph_add_node(struct computation_graph *graph,
     new_node->n_children = 0;
     new_node->updated = 1;
     new_node->output_values = malloc(type->n_outputs * sizeof(double));
-    new_node->param_values = malloc(type->n_params * sizeof(double));
+    new_node->param_values = calloc(type->n_params, sizeof(double));
     new_node->input_srcs = malloc(type->n_inputs * sizeof(struct input_type));
     // Check that malloc succeeded in every case which memory was requested
     if ((type->n_outputs && !new_node->output_values) ||

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/PID.h

@@ -49,6 +49,7 @@
 #define YPOS_KP 0.015f
 #define YPOS_KI 0.005f
 #define YPOS_KD 0.03f
+#define YPOS_ALPHA 0
 
 
 //Pitch constants
@@ -74,6 +75,7 @@
 #define XPOS_KP -0.015f
 #define XPOS_KI -0.005f
 #define XPOS_KD -0.03f
+#define XPOS_ALPHA 0
 
 
 //Throttle constants

quad/sw/modular_quad_pid/src/control_algorithm.c

@@ -174,9 +174,11 @@ int control_algorithm_init(parameter_t * ps)
     graph_set_param_val(graph, ps->x_pos_pid, PID_KP, XPOS_KP);
     graph_set_param_val(graph, ps->x_pos_pid, PID_KI, XPOS_KI);
     graph_set_param_val(graph, ps->x_pos_pid, PID_KD, XPOS_KD);
+    graph_set_param_val(graph, ps->x_pos_pid, PID_ALPHA, XPOS_ALPHA);
     graph_set_param_val(graph, ps->y_pos_pid, PID_KP, YPOS_KP);
     graph_set_param_val(graph, ps->y_pos_pid, PID_KI, YPOS_KI);
     graph_set_param_val(graph, ps->y_pos_pid, PID_KD, YPOS_KD);
+    graph_set_param_val(graph, ps->y_pos_pid, PID_ALPHA, YPOS_ALPHA);
     graph_set_param_val(graph, ps->alt_pid, PID_KP, ALT_ZPOS_KP);
     graph_set_param_val(graph, ps->alt_pid, PID_KI, ALT_ZPOS_KI);
     graph_set_param_val(graph, ps->alt_pid, PID_KD, ALT_ZPOS_KD);
@@ -189,6 +191,10 @@ int control_algorithm_init(parameter_t * ps)
     graph_set_param_val(graph, ps->clamp_d_pwmY, BOUNDS_MIN, -PWM_DIFF_BOUNDS);
     graph_set_param_val(graph, ps->clamp_d_pwmY, BOUNDS_MAX, PWM_DIFF_BOUNDS);
 
+
+    // Initial value for sampling periods
+    graph_set_param_val(graph, ps->pos_time, CONST_SET, 0.04);
+    graph_set_param_val(graph, ps->angle_time, CONST_SET, 0.005);
     // Set initial mode
     connect_manual(ps);
 
@@ -243,7 +249,7 @@ int control_algorithm_init(parameter_t * ps)
 		user_defined_struct->flight_mode = AUTO_FLIGHT_MODE;
 		connect_autonomous(ps);
 		// Reset this when autonomous is engaged, so there is not a large difference at the start of autonomous
-		last_vrpn_id = sensor_struct->currentQuadPosition.packetId;
+		last_vrpn_id = sensor_struct->currentQuadPosition.packetId - 1;
 	}
 
 	//PIDS///////////////////////////////////////////////////////////////////////

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,

quad/sw/modular_quad_pid/src/graph_blocks/node_pid.c

@@ -8,6 +8,7 @@ static double FLT_EPSILON = 0.0001;
 struct pid_node_state {
     double prev_error; // Previous error
     double acc_error; // Accumulated error
+    double last_filtered; // Last output from the filtered derivative
 };
 
 // The generic PID diagram. This function takes in pid inputs (CUR_POINT and SETOINT) and calculates the output "pid_correction"
@@ -49,13 +50,20 @@ static void pid_computation(void *state, const double* params, const double *inp
       pid_state->acc_error += error;
     }
 
-    double change_in_error = error - pid_state->prev_error;
 
     // Compute each term's contribution
     P = params[PID_KP] * error;
     I = params[PID_KI] * pid_state->acc_error * inputs[PID_DT];
-    D = params[PID_KD] * (change_in_error / inputs[PID_DT]);
-
+    // Low-pass filter on derivative
+    double change_in_error = error - pid_state->prev_error;
+    double term1 = params[PID_ALPHA] * pid_state->last_filtered;
+    double derivative = change_in_error / inputs[PID_DT];
+    if (inputs[PID_DT] == 0) { // Divide by zero check
+        derivative = 0;
+    }
+    double term2 = params[PID_KD] * (1.0f - params[PID_ALPHA]) * derivative;
+    D = term1 + term2;
+    pid_state->last_filtered = D; // Store filtered value for next filter iteration
     pid_state->prev_error = error; // Store the current error into the state
 
     outputs[PID_CORRECTION] = P + I + D; // Store the computed correction
@@ -67,19 +75,20 @@ static void reset_pid(void *state) {
     struct pid_node_state* pid_state = (struct pid_node_state*)state;
     pid_state->acc_error = 0;
     pid_state->prev_error = 0;
+    pid_state->last_filtered = 0;
 }
 
 
 static const char* const in_names[3] = {"Cur point", "Setpoint", "dt"};
 static const char* const out_names[1] = {"Correction"};
-static const char* const param_names[3] = {"Kp", "Ki", "Kd"};
+static const char* const param_names[4] = {"Kp", "Ki", "Kd", "alpha"};
 const struct graph_node_type node_pid_type = {
         .input_names = in_names,
         .output_names = out_names,
         .param_names = param_names,
         .n_inputs = 3,
         .n_outputs = 1,
-        .n_params = 3,
+        .n_params = 4,
         .execute = pid_computation,
         .reset = reset_pid
 };

quad/sw/modular_quad_pid/src/graph_blocks/node_pid.h

@@ -19,7 +19,8 @@ enum graph_node_pid_outputs {
 enum graph_node_pid_params {
     PID_KP, // Proportional constant
     PID_KI, // Integral constant
-    PID_KD // Derivative constant
+    PID_KD, // Derivative constant
+    PID_ALPHA // alpha = (1 - N*T_s); High values mean more filtering
 };
 
 #endif // __NODE_PID_H__