Something went wrong on our end
-
Andy Snawerdt authoredAndy Snawerdt authored
modelParameters.m 7.09 KiB
% 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;
% Model Parameters
m = 1.244; % Quadrotor + battery mass
g = 9.81; % Acceleration of gravity
Jxx = 0.0130; % Quadrotor and battery motor of inertia around bx (pitch)
Jyy = 0.0140; % Quadrotor and battery motor of inertia around by (roll)
Jzz = 0.0285; % Quadrotor and battery motor of inertia around bz (yaw)
Jreq = 4.2012e-05; % Rotor and motor moment of inertia around axis of rotation
Kt = 8.6519e-6; % Rotor thrust constant
Kh = 0; % Rotor in-plane drag constant
Kd = 1.0317e-7; % Rotor drag constant
rhx = 0.16; % X-axis distance from center of mass to a rotor hub
rhy = 0.16; % Y-axis distance from center of mass to a rotor hub
rhz = 0.03; % Z-axis distance from center of mass to a rotor hub
Rm = 0.2308; % Motor resistance
Kq = 96.3422; % Motor torque constant
Kv = 96.3422; % Motor back emf constant
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.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
y_controlled_o = 0; % Equilibrium longitudinal controller output
yaw_controlled_o = 0; % Equilibrium yaw controller output
% 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 + ...
+ (((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;
if logAnalysisToggle == 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 = '';
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;
time = time - time(1);
time = time(1):0.005:time(length(time)-7);
time = time';
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.Alt_Setpoint_Constant.data;
z_position = dataStruct.VRPN_Alt_Constant.data;
z_error = timeseries(z_setpoint - z_position, time);
% Determine pitch error
pitch_setpoint = dataStruct.X_pos_PID_Correction.data;
pitch_value = dataStruct.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.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.data;
y_command = dataStruct.Roll_Rate_PID_Correction.data;
z_command = dataStruct.Altitude_PID_Correction.data;
yaw_command = dataStruct.Yaw_Rate_PID_Correction.data;
% 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;
%Pull the measurements from the acceleratometer
raw_accel_data_x = dataStruct.accel_x.data;
raw_accel_data_y = dataStruct.accel_y.data;
raw_accel_data_z = dataStruct.accel_z.data;
raw_accel_data_arr = ...
[ raw_accel_data_x , raw_accel_data_y , raw_accel_data_z ];
raw_accel_data = timeseries( raw_accel_data_arr , time );
%Pull the measurements from the gyroscope
raw_gyro_data_x = dataStruct.gyro_x.data;
raw_gyro_data_y = dataStruct.gyro_y.data;
raw_gyro_data_z = dataStruct.gyro_z.data;
raw_gyro_data_arr = ...
[ raw_gyro_data_x , raw_gyro_data_y , raw_gyro_data_z ];
raw_gyro_data = timeseries( raw_gyro_data_arr , time );
%Create time series object for z command
throttle_command = timeseries(dataStruct.RC_Throttle_Constant.data, time);
%Pull the measurements from the complimentary filter
pitch_measured_IMU = dataStruct.Pitch_Constant.data;
roll_measured_IMU = dataStruct.Roll_Constant.data;
end