From 5521309d754e93a371c9d047bb4154cc5d3ebfbe Mon Sep 17 00:00:00 2001
From: ucart <ucart_groundstation@iastate.edu>
Date: Sat, 15 Apr 2017 00:15:25 -0500
Subject: [PATCH] adding alpha to log, updating matlab scripts
---
controls/DataAnalysisTool/Tool/DataAnalysis.m | 2 +-
controls/DataAnalysisTool/Tool/simplePlots.m | 71 ++++++++++++++++++-
quad/src/quad_app/log_data.c | 5 +-
3 files changed, 73 insertions(+), 5 deletions(-)
diff --git a/controls/DataAnalysisTool/Tool/DataAnalysis.m b/controls/DataAnalysisTool/Tool/DataAnalysis.m
index 5cd77413d..a7b873173 100644
--- a/controls/DataAnalysisTool/Tool/DataAnalysis.m
+++ b/controls/DataAnalysisTool/Tool/DataAnalysis.m
@@ -12,7 +12,7 @@ fname = '';
% PLOTTING SWITCHES - set them to 0 or 1 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
-plot = 1; % to choose plotting
+plot = 0; % to choose plotting
separatePlot = 1; % to generate separatePlots
multiPlot = 1; % to generate multiPlot
subPlot = 1; % to generate subPlots
diff --git a/controls/DataAnalysisTool/Tool/simplePlots.m b/controls/DataAnalysisTool/Tool/simplePlots.m
index 57d029fc2..90bef7420 100644
--- a/controls/DataAnalysisTool/Tool/simplePlots.m
+++ b/controls/DataAnalysisTool/Tool/simplePlots.m
@@ -57,6 +57,29 @@ plot(expData.Time.data, expData.gyro_y.data .* 100000);
legend('output', 'Pitch rate x100000');
title('pitch rate output');
+linkaxes([ax1, ax2, ax3, ax4], 'x');
+%% X pos controller flow
+figure;
+ax2 = subplot(2,2,1);
+plot(expData.Time.data, expData.X_Setpoint_Constant.data - expData.OF_Integrate_X_Integrated.data);
+title('X error');
+
+ax1 = subplot(2,2,2);
+plot(expData.Time.data, expData.X_pos_PID_Correction.data);
+title('x output');
+
+ax3 = subplot(2,2,3);
+plot(expData.Time.data, expData.Pitch_PID_Correction.data); hold on;
+plot(expData.Time.data, expData.VRPN_Pitch_Constant.data .* 10);
+title('pitch output');
+legend('output', 'Pitch x10');
+
+ax4 = subplot(2,2,4);
+plot(expData.Time.data, expData.Pitch_Rate_PID_Correction.data); hold on;
+plot(expData.Time.data, expData.gyro_y.data);
+legend('output', 'Pitch rate');
+title('pitch rate output');
+
linkaxes([ax1, ax2, ax3, ax4], 'x');
%%
plot(time, 1044.26 .* (PitchPIDCorrection - gyro_y));hold on;
@@ -70,8 +93,8 @@ plot(time, YawRatePIDCorrection);
linkaxes([ax1, ax2], 'x');
%%
-all_motors = expData.Signal_Mixer_PWM_0.data + expData.Signal_Mixer_PWM_1.data + ...
- expData.Signal_Mixer_PWM_2.data + expData.Signal_Mixer_PWM_3.data;
+all_motors = expData.Signal_Mixer_MOTOR_0.data + expData.Signal_Mixer_MOTOR_1.data + ...
+ expData.Signal_Mixer_MOTOR_2.data + expData.Signal_Mixer_MOTOR_3.data;
ax1 = subplot(1, 2, 1);
plot(expData.Time.data, all_motors ./ 4); hold on;
plot(expData.Time.data, expData.RC_Throttle_Constant.data); hold on;
@@ -112,6 +135,7 @@ ax3 = subplot(3, 1, 3);
plot(expData.Time.data, expData.gyro_z.data);
linkaxes([ax1, ax2, ax3], 'x');
%%
+figure;
ax2 = subplot(2,2,1);
raw_derivative = -diff(expData.VRPN_X_Constant.data) / 0.04;
plot(expData.Time.data, expData.X_Vel_Correction.data - (expData.RC_Pitch_Constant.data * 5)); hold on;
@@ -136,7 +160,31 @@ legend('output', 'Pitch rate x100000');
title('pitch rate output');
linkaxes([ax1, ax2, ax3, ax4], 'x');
+%% vel flow
+figure;
+ax2 = subplot(2,2,1);
+plot(expData.Time.data, expData.OF_Integrate_X_Integrated.data - expData.X_pos_PID_Correction.data); hold on;
+%plot(expData.Time.data, expData.X_Vel_Correction.data); hold on;
+%plot(expData.Time.data, [0; raw_derivative]);
+title('X velocity error');
+
+ax1 = subplot(2,2,2);
+plot(expData.Time.data, expData.X_Vel_PID_Correction.data);
+title('x vel output');
+
+ax3 = subplot(2,2,3);
+plot(expData.Time.data, expData.Pitch_PID_Correction.data); hold on;
+plot(expData.Time.data, expData.VRPN_Pitch_Constant.data .* 10);
+title('pitch output');
+legend('output', 'Pitch x10');
+
+ax4 = subplot(2,2,4);
+plot(expData.Time.data, expData.Pitch_Rate_PID_Correction.data); hold on;
+plot(expData.Time.data, expData.gyro_y.data .* 100000);
+legend('output', 'Pitch rate x100000');
+title('pitch rate output');
+linkaxes([ax1, ax2, ax3, ax4], 'x');
%%
ax1 = subplot(2, 1, 1);
plot(expData.Time.data, expData.Alt_Setpoint_Constant.data - expData.VRPN_Alt_Constant.data); hold on;
@@ -178,3 +226,22 @@ plot(expData.Time.data, expData.Altitude_PID_Correction.data);
linkaxes([ax1, ax2], 'x');
%%
+figure;
+ax1 = subplot(2, 1, 1);
+plot(expData.Time.data, expData.OF_Integrate_X_Integrated.data); hold on;
+plot(expData.Time.data, expData.VRPN_X_Constant.data);
+legend('OF X Position', 'VRPN X Position');
+xlabel('Time (s)');
+ylabel('Displacement (m)');
+hold off;
+
+ax2 = subplot(2, 1, 2);
+plot(expData.Time.data, expData.OF_Integrate_Y_Integrated.data); hold on;
+plot(expData.Time.data, expData.VRPN_Y_Constant.data);
+legend('OF Y Position', 'VRPN Y Position');
+xlabel('Time (s)');
+ylabel('Displacement (m)');
+hold off;
+
+linkaxes([ax1, ax2]);
+
diff --git a/quad/src/quad_app/log_data.c b/quad/src/quad_app/log_data.c
index bdc38f0ee..a2f7e62d6 100644
--- a/quad/src/quad_app/log_data.c
+++ b/quad/src/quad_app/log_data.c
@@ -199,11 +199,12 @@ void printLogging(hardware_t *hardware_struct, log_t* log_struct, parameter_t* p
for (i = 0; i < ps->graph->n_nodes; ++i) {
struct graph_node* node = &ps->graph->nodes[i];
if (node->type->type_id == BLOCK_PID) {
- double kp, ki, kd;
+ double kp, ki, kd, alpha;
kp = graph_get_param_val(ps->graph, i, 0);
ki = graph_get_param_val(ps->graph, i, 1);
kd = graph_get_param_val(ps->graph, i, 2);
- safe_sprintf_cat(&buf, "# %s :\tKp = %lf Ki = %lf Kd = %lf\n", node->name, kp, ki, kd);
+ alpha = graph_get_param_val(ps->graph, i, 3);
+ safe_sprintf_cat(&buf, "# %s :\tKp = %lf Ki = %lf Kd = %lf Alpha = %lf\n", node->name, kp, ki, kd, alpha);
}
}
--
GitLab