diff --git a/controls/DataAnalysisTool/Tool/DataAnalysis.m b/controls/DataAnalysisTool/Tool/DataAnalysis.m index 5cd77413d324f07efd498f48265e068b096ef1b3..a7b87317346c0d2289901a5cefe5c6aec86e4ca8 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 57d029fc28f20bb4685eb9d4c405fd29550bab59..90bef74208356164e30ab0ad7811f35db554fdb2 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 bdc38f0eeb05714f73d334ea492ee8c6311abf21..a2f7e62d6e3302b1fb5a1b8f846538a572c4ed11 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); } }