diff --git a/controls/model/loggingAnalysis/logAnalysis.m b/controls/model/loggingAnalysis/logAnalysis.m
index d713a65bc2e51180c5078de694ef94f3bfa6f53b..ae48f90734c220bf920b8367ca5fca88411d84c7 100644
--- a/controls/model/loggingAnalysis/logAnalysis.m
+++ b/controls/model/loggingAnalysis/logAnalysis.m
@@ -31,6 +31,12 @@ yawrate_setpoint_model_data = yawrate_setpoint_model.signals.values(indices_40ms
yaw_command_model_data = yaw_command_model.signals.values(indices_40ms);
yaw_value_model_data = yaw_value_model.signals.values(indices_40ms);
+% Pull duty cycle commands from model
+PWM0_model = motorCommands.signals.values(indices_40ms, 1);
+PWM1_model = motorCommands.signals.values(indices_40ms, 2);
+PWM2_model = motorCommands.signals.values(indices_40ms, 3);
+PWM3_model = motorCommands.signals.values(indices_40ms, 4);
+
%% Plot x control structure
% Plot lateral controller output
@@ -154,4 +160,37 @@ stairs(time_model_40ms, yaw_value_model_data, '.-'); hold off;
title('Yaw Position');
xlabel('Time (s)');
ylabel('Value (rad)');
+legend('Log', 'Model', 'location', 'northwest');
+
+%% Plot PWM Commands
+figure(5); subplot(2, 2, 1);
+stairs(time, PWM0,'.-'); hold on; grid minor;
+stairs(time_model_40ms, PWM0_model, '.-'); hold off;
+title('PWM0 Value');
+xlabel('Time (s)');
+ylabel('PWM0 Command');
+legend('Log', 'Model', 'location', 'northwest');
+
+subplot(2, 2, 2);
+stairs(time, PWM1,'.-'); hold on; grid minor;
+stairs(time_model_40ms, PWM1_model, '.-'); hold off;
+title('PWM1 Value');
+xlabel('Time (s)');
+ylabel('PWM1 Command');
+legend('Log', 'Model', 'location', 'northwest');
+
+subplot(2, 2, 3);
+stairs(time, PWM2,'.-'); hold on; grid minor;
+stairs(time_model_40ms, PWM2_model, '.-'); hold off;
+title('PWM2 Value');
+xlabel('Time (s)');
+ylabel('PWM2 Command');
+legend('Log', 'Model', 'location', 'northwest');
+
+subplot(2, 2, 4);
+stairs(time, PWM3,'.-'); hold on; grid minor;
+stairs(time_model_40ms, PWM3_model, '.-'); hold off;
+title('PWM3 Value');
+xlabel('Time (s)');
+ylabel('PWM3 Command');
legend('Log', 'Model', 'location', 'northwest');
\ No newline at end of file
diff --git a/controls/model/modelParameters.m b/controls/model/modelParameters.m
index 9ed5d14109548d4115835d454df256cc834e82f8..97115a7c498158b6e5d7765cb3de075772fee193 100644
--- a/controls/model/modelParameters.m
+++ b/controls/model/modelParameters.m
@@ -1,6 +1,7 @@
+temp = 0;
% Log Analysis Toggle
logAnalysisToggle = 1; % 1 for log analysis, 0 for normal operation
-
+
% Define Simulink Runtime (if logAnalysisToggle is selected, this will be
% automatically set based on the log files time)
runtime = 20;
@@ -31,23 +32,50 @@
x_controlled_o = 0; % Equilibrium lateral controller output
y_controlled_o = 0; % Equilibrium longitudinal controller output
yaw_controlled_o = 0; % Equilibrium yaw controller output
- omega_o = sqrt((m*g)/(4*Kt)); % Equilibrium Rotor Speed
+
+ % Determine Initial Conditions
+
+ % Equilibrium rotor speeds
+ omega0_o = sqrt((m*g)/(4*Kt));
+ omega1_o = sqrt((m*g)/(4*Kt));
+ omega2_o = sqrt((m*g)/(4*Kt));
+ omega3_o = sqrt((m*g)/(4*Kt));
% Equilibrium height controller output
height_controlled_o = (((Rm*If + ...
- + (((omega_o * 2 * Rm * Kv * Kq ...
+ + (((omega0_o * 2 * Rm * Kv * Kq ...
* Kd + 1)^2) - 1)/(4* Rm*Kv^2*Kq ...
*Kd))/Vb)*(Pmax- Pmin)+Pmin);
+ % Equilibrium positions
+ x_o = 0;
+ y_o = 0;
+ z_o = 0;
+
+ % Equilibrium velocities
+ x_vel_o = 0;
+ y_vel_o = 0;
+ z_vel_o = 0;
+
+ % Equilibrium angles
+ roll_o = 0;
+ pitch_o = 0;
+ yaw_o = 0;
+
+ % Equilibrium angular rates
+ rollrate_o = 0;
+ pitchrate_o = 0;
+ yawrate_o = 0;
+
% Import Data and determine errors
-if logAnalysisToggle == 1
+if logAnalysisToggle == 1 && temp == 0
% Import Data to Workspace
data = importdata('loggingAnalysis/logFiles/logData.csv');
% Set up time vector
time = data.data(:, 1);
runtime = max(time);
-
+
% Determine x position error
x_setpoint = data.data(:, 25);
x_position = data.data(:, 20);
@@ -75,7 +103,7 @@ if logAnalysisToggle == 1
% Determine yaw error
yaw_setpoint = zeros(length(time), 1); % NEEDS UPDATED WHEN LOG FILE INCLUDES YAW SETPOINT
- yaw_value = ones(length(time), 1); % NEEDS UPDATED WHEN LOG FILE INCLUDES YAW VALUE
+ yaw_value = data.data(:,10);%(length(time), 1); % NEEDS UPDATED WHEN LOG FILE INCLUDES YAW VALUE
yaw_error = timeseries(yaw_setpoint - yaw_value, time);
% Determine pitch rate error
@@ -98,4 +126,134 @@ if logAnalysisToggle == 1
y_command = data.data(:, 15);
z_command = data.data(:, 8);
yaw_command = data.data(:, 16);
+
+ % Determine signal mix PWM values
+ PWM0 = data.data(:, 28);
+ PWM1 = data.data(:, 29);
+ PWM2 = data.data(:, 30);
+ PWM3 = data.data(:, 31);
+
+ % Determine the initial height controller command
+ % height_controlled_o = 1; % NEEDS UPDATED WHEN LOG FILES INCLUDE THROTTLE COMMAND
+
+ % Determine the initial rotor speeds based on PWM inputs
+ u_P0 = (PWM0(1) - Pmin) / (Pmax - Pmin);
+ u_P1 = (PWM1(1) - Pmin) / (Pmax - Pmin);
+ u_P2 = (PWM2(1) - Pmin) / (Pmax - Pmin);
+ u_P3 = (PWM3(1) - Pmin) / (Pmax - Pmin);
+
+ Vb_eff_0 = u_P0 * Vb;
+ Vb_eff_1 = u_P1 * Vb;
+ Vb_eff_2 = u_P2 * Vb;
+ Vb_eff_3 = u_P3 * Vb;
+
+ omega0_o = (-1 + sqrt(1 - 4*Rm*Kv*Kq*Kd*( Kv*Rm*If - Kv*Vb_eff_0))) / (2*Rm*Kv*Kq*Kd);
+ omega1_o = (-1 + sqrt(1 - 4*Rm*Kv*Kq*Kd*( Kv*Rm*If - Kv*Vb_eff_1))) / (2*Rm*Kv*Kq*Kd);
+ omega2_o = (-1 + sqrt(1 - 4*Rm*Kv*Kq*Kd*( Kv*Rm*If - Kv*Vb_eff_2))) / (2*Rm*Kv*Kq*Kd);
+ omega3_o = (-1 + sqrt(1 - 4*Rm*Kv*Kq*Kd*( Kv*Rm*If - Kv*Vb_eff_3))) / (2*Rm*Kv*Kq*Kd);
+
+ % Determine initial positions for x, y, and z
+ x_o = x_position(1);
+ y_o = y_position(1);
+ z_o = z_position(1);
+
+ % Determine initial velocity for x, y, and z
+ x_vel_o = (x_position(2) - x_position(1)) / (time(2) - time(1));
+ y_vel_o = (y_position(2) - y_position(1)) / (time(2) - time(1));
+ z_vel_o = (z_position(2) - z_position(1)) / (time(2) - time(1));
+
+ % Determine initial angles
+ roll_o = roll_value(1);
+ pitch_o = pitch_value(1);
+ yaw_o = yaw_value(1);
+
+ % Determine initial angular rates
+ rollrate_o = rollrate_value(1);
+ pitchrate_o = pitchrate_value(1);
+ yawrate_o = yawrate_value(1);
+
+elseif logAnalysisToggle == 1 && temp == 1
+ %%%%%% Commented out section until logging is .txt file based %%%%%%
+ % FNAME
+ % if you know the name of the log file that you want to parse, set the it
+ % here only if it is in the working directory. Otherwise, you may leave it
+ % blank. You will be able to choose the file to parse through an explorer
+ % window.
+ %
+ %fname = 'sampleLogFile.txt';
+ fname = 'sampleLogFile.txt';
+ fpath = '';
+
+ if(isempty(fname))
+ [fname, fpath] = uigetfile('.txt','Select log file');
+ end
+
+ params.file.name = fname; % file name only
+ params.file.path = fpath; % file path only
+ params.file.pathName = [fpath fname]; % file path with file name
+
+ [dataStruct, headers] = parse_log_model(params.file.pathName);
+
+ time = dataStruct.Time.data;
+ runtime = max(time);
+
+ % Determine x position error
+ x_setpoint = dataStruct.X_Setpoint_Constant.data;
+ x_position = dataStruct.VRPN_X_Constant.data;
+ x_error = timeseries(x_setpoint - x_position, time);
+
+ % Determine y position error
+ y_setpoint = dataStruct.Y_Setpoint_Constant.data;
+ y_position = dataStruct.VRPN_Y_Constant.data;
+ y_error = timeseries(y_setpoint - y_position, time);
+
+ % Determine z position error
+ z_setpoint = dataStruct.Z_Setpoint_Constant.data;
+ z_position = dataStruct.VRPN_Z_Constant.data;
+ z_error = timeseries(z_setpoint - z_position, time);
+
+ % Determine pitch error
+ pitch_setpoint = dataStruct.X_pos_PID_Correction.data;
+ pitch_value = dataStruct.VRPN_Pitch_Constant.data;
+ pitch_error = timeseries(pitch_setpoint - pitch_value, time);
+
+ % Determine roll error
+ roll_setpoint = dataStruct.Y_pos_PID_Correction.data;
+ roll_value = dataStruct.VRPN_Roll_Constant.data;
+ roll_error = timeseries(roll_setpoint - roll_value, time);
+
+ % Determine yaw error
+ yaw_setpoint = zeros(length(time), 1); % NEEDS UPDATED WHEN LOG FILE INCLUDES YAW SETPOINT
+ yaw_value = zeros(length(time), 1); % NEEDS UPDATED WHEN LOG FILE INCLUDES YAW VALUE
+ yaw_error = timeseries(yaw_setpoint - yaw_value, time);
+
+ % Determine pitch rate error
+ pitchrate_setpoint = dataStruct.Pitch_PID_Correction.data;
+ pitchrate_value = dataStruct.gyro_y.data;
+ pitchrate_error = timeseries(pitchrate_setpoint - pitchrate_value, time);
+
+ % Determine roll rate error
+ rollrate_setpoint = dataStruct.Roll_PID_Correction.data;
+ rollrate_value = dataStruct.gyro_x.data;
+ rollrate_error = timeseries(rollrate_setpoint - rollrate_value, time);
+
+ % Determine yaw rate error
+ yawrate_setpoint = dataStruct.Yaw_PID_Correction.data;
+ yawrate_value = dataStruct.gyro_z.data;
+ yawrate_error = timeseries(yawrate_setpoint - yawrate_value, time);
+
+ % Pull motor commands from log
+ x_command = dataStruct.Pitch_Rate_PID_Correction;
+ y_command = dataStruct.Roll_Rate_PID_Correction;
+ z_command = dataStruct.Altitude_PID_Correction;
+ yaw_command = dataStruct.Yaw_Rate_PID_Correction;
+
+ % Determine signal mix PWM values
+ PWM0 = dataStruct.Signal_Mixer_PWM_0.data;
+ PWM1 = dataStruct.Signal_Mixer_PWM_1.data;
+ PWM2 = dataStruct.Signal_Mixer_PWM_2.data;
+ PWM3 = dataStruct.Signal_Mixer_PWM_3.data;
+
end
+
+
diff --git a/controls/model/parse_log_model.m b/controls/model/parse_log_model.m
new file mode 100644
index 0000000000000000000000000000000000000000..9fba92fa6d0d3d670370b06a035fd9ee802698d2
--- /dev/null
+++ b/controls/model/parse_log_model.m
@@ -0,0 +1,111 @@
+function [loggedData, headers] = parse_log_model(filename, params, expData)
+%parse_log This independent function parses the data stored in the file and
+%returns a structure containing the data
+% filename - this is the complete path of the file with the filename
+% params - this is the params structure which holds the analysis
+% configuration options
+
+% Check if file exists
+if (~exist(filename,'file'))
+ error(strcat(filename, ' does not exist'));
+end
+
+% Open file
+FileID = fopen(filename, 'r');
+
+% Gets the first line of the file
+string = fgetl(FileID);
+
+% Test first line, if not formatted correctly, reject
+if(size(regexp(string, '^#')) == 0)
+ error(strcat(filename, ' is not properly formatted, and does not contain "#" headers'));
+end
+
+% Loop through header lines
+while( regexp(string, '^#') == 1 )
+
+ %Print out string and move on
+ disp(string)
+ old = string;
+ string = fgetl(FileID);
+
+end
+
+% Two possibilities for the next two lines:
+% 1) line of headers
+% 2) line of units
+foundHeaders = 0;
+foundUnits = 0;
+
+% Checking current line's type:
+identifier = string(1);
+
+if (regexp(identifier,'%'))
+ foundHeaders = 1;
+ % this is a line of headers; extract headers:
+ headers = strsplit(string);
+ headers{1} = strrep(headers{1},'%', '');
+ numOfHeaders = length(headers);
+else
+ if (regexp(identifier,'&'))
+ foundUnits = 1;
+ % this is a line of units; extract units:
+ units = strsplit(string);
+ units{1} = strrep(units{1},'&','');
+ else
+ error(strcat(filename, ' is not properly formatted, contains undefined line identifier.'));
+ end
+end
+
+% Obtaining the next line
+string = fgetl(FileID);
+identifier = string(1);
+
+if(foundHeaders)
+ if(regexp(identifier,'&'))
+ foundUnits = 1;
+ % this is a line of units; extract units:
+ units = strsplit(string);
+ units{1} = strrep(units{1},'&','');
+ else
+ error(strcat(filename, ' is not properly formatted, contains or undefined/excessive line identifiers.'));
+ end
+else
+ if(foundUnits)
+ if(regexp(identifier,'%'))
+ % this is a line of headers; extract headers:
+ headers = strsplit(string);
+ headers{1} = strrep(headers{1},'%', '');
+ numOfHeaders = length(headers);
+ end
+ else
+ error('Should never be able to get here');
+ end
+end
+
+% sanity check and clean up
+if(numOfHeaders ~= length(units))
+ error(strcat(filename, ' is not properly formatted, contains unmatched number of units and headers'));
+end
+clear foundHeaders foundUnits;
+
+% Get all data into a single matrix called "log"
+log = [];
+line = zeros(1,numOfHeaders);
+while ~feof(FileID)
+ line = textscan(FileID, '%f', numOfHeaders);
+ line = transpose(cell2mat(line));
+ log = [log;line];
+end
+
+% Converting the log matrix into a expData structure.
+for i = 1:numOfHeaders
+
+ eval(['loggedData.' headers{i} '.data = log(:,i);']); % adding data
+ eval(['loggedData.' headers{i} '.unit = cell2mat(units(i));']); % adding unit
+
+end
+
+
+end
+
diff --git a/controls/model/test_model.slx b/controls/model/test_model.slx
index 580cbfee5ee130885a96fc9f254099058bfb5e41..56975e7eba373b73a005d9451ddcb553bd36f882 100644
Binary files a/controls/model/test_model.slx and b/controls/model/test_model.slx differ