Added ability to use logged data as inputs to model for model comparison

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

controls/model/modelParameters.m

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

controls/model/parse_log_model.m

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