Skip to content
Snippets Groups Projects
Commit a1c0e10a authored by bapries's avatar bapries
Browse files

Added necesarry files from andy for the simulink model to work

parent 8164fb57
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
controls/model/Quadcopter_Model_R2016_B.slx
+ 0
− 0
View file @ a1c0e10a
No preview for this file type
controls/model/modelParameters.m
+ 84
− 65
View file @ a1c0e10a
% Add Library Blocks to Path
path = genpath('Library_blocks');
addpath(path);
% Add Library Blocks and 3D animation to Path
path_1 = genpath('Library_blocks');
path_2 = genpath('3D_Animation');
addpath(path_1, path_2);
% Log Analysis Toggle
logAnalysisToggle = 1; % 1 for log analysis, 0 for normal operation
logAnalysisToggle = 0; % 1 for log analysis, 0 for normal operation
% Physics Verification Toggle
physicsVerificationToggle = 1;
% Define Simulink Runtime (if logAnalysisToggle is selected, this will be
% automatically set based on the log files time)
runtime = 55;
runtime = 40;%max(time);
% Model Parameters
m = 1.216; % 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 = 1.2007e-05; % Rotor thrust constant
Kh = 3.4574e-04; % Rotor in-plane drag constant
Kd = 1.4852e-07; % 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
r_oc = [0; 0; 0]; % Vector from origin to center of mass
Rm = 0.235; % Motor resistance
Kq = 96.3422; % Motor torque constant
Kv = 96.3422; % Motor back emf constant
If = 0.3836; % Motor internal friction current
Pmin = 0; % Minimum zybo output duty cycle command
Pmax = 1; % 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 = 12.3; % 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
Kp_mahony = 0.4; % Proportional term for mahony filter
Ki_mahony = 0.001; % Integral term for mahony filter
delta_T = 4 * [ 0, 0, 2.351e-4 ];
m = 1.216; % Quadrotor + battery mass
g = 9.81; % Acceleration of gravity
Jxx = 0.0110;%0.0130; % Quadrotor and battery motor of inertia around bx (pitch)
Jyy = 0.0116;%0.0140; % Quadrotor and battery motor of inertia around by (roll)
Jzz = 0.0223;%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 = 1.2007e-05; % Rotor thrust constant
delta_T = [ 0, 0, 9.404e-04 ]; % Thrust constant adjustment factor
Kh = 3.4574e-04; % Rotor in-plane drag constant
Kd = 1.4852e-07; % 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
r_oc = [0; 0; 0]; % Vector from origin to center of mass
Rm = 0.235; % Motor resistance
Kq = 96.3422; % Motor torque constant
Kv = 96.3422; % Motor back emf constant
If = 0.3836; % Motor internal friction current
Pmin = 0; % Minimum zybo output duty cycle command
Pmax = 1; % Maximum zybo output duty cycle command
Tc = 0.01;%0.04; % Camera system sampling period
Tq = 0.005; % Quad sampling period
tau_c = 0; % Camera system total latency
Vb = 12.3; % 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
Kp_mahony = 0.4; % Proportional term for mahony filter
Ki_mahony = 0.001; % Integral term for mahony filter
% Define Biquad Filter Parameters
b0 = 0.020083;
b1 = 0.040167;
b2 = 0.020083;
a0 = 1;
a1 = -1.561;
a2 = 0.6414;
SOS_Matrix = [b0, b1, b2, a0, a1, a2];
accel_Fc = 10;
Fs = 1/Tq;
accel_K = tan(pi * accel_Fc / Fs);
Q = 0.707;
accel_norm = 1.0 / (1.0 + accel_K/Q + accel_K*accel_K);
% Determine Initial Conditions
accel_b0 = accel_K*accel_K*accel_norm;
accel_b1 = 2.0*accel_b0;
accel_b2 = accel_b0;
accel_a0 = 1;
accel_a1 = 2.0*(accel_K*accel_K -1) * accel_norm;
accel_a2 = (1.0 - accel_K/Q + accel_K*accel_K) * accel_norm;
% 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));
accel_SOS_Matrix = [accel_b0, accel_b1, accel_b2, accel_a0, accel_a1, accel_a2];
% 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);
OF_Fc = 7;
OF_K = tan(pi * OF_Fc / Fs);
OF_norm = 1.0 / (1.0 + OF_K/Q + OF_K*OF_K);
OF_b0 = OF_K*OF_K*OF_norm;
OF_b1 = 2.0*OF_b0;
OF_b2 = OF_b0;
OF_a0 = 1;
OF_a1 = 2.0*(OF_K*OF_K -1) * OF_norm;
OF_a2 = (1.0 - OF_K/Q + OF_K * OF_K) * OF_norm;
OF_SOS_Matrix = [OF_b0, OF_b1, OF_b2, OF_a0, OF_a1, OF_a2];
% Determine Initial Conditions
% Equilibrium positions
% Position Initial Conditions
x_o = 0;
y_o = 0;
z_o = 0;
% Equilibrium velocities
% Velocity Initial Conditions
x_vel_o = 0;
y_vel_o = 0;
z_vel_o = 0;
% Equilibrium angles
% Euler Angle Initial Conditions
roll_o = 0;
pitch_o = 0;
yaw_o = 0;
% Equilibrium angular rates
% Euler Rate Initial Conditions
rollrate_o = 0;
pitchrate_o = 0;
yawrate_o = 0;
% 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);
if logAnalysisToggle == 1
%%%%%% Commented out section until logging is .txt file based %%%%%%
% FNAME
......@@ -96,10 +118,10 @@ if logAnalysisToggle == 1
%
%fname = 'sampleLogFile.txt';
fname = '';
fpath = '';
fpath = 'C:\Users\Andy\Documents\School\MicroCART\GitRepo\MicroCART_17-18\controls\model\logFiles\';
if(isempty(fname))
[fname, fpath] = uigetfile('.txt','Select log file');
[fname] = uigetfile('.txt','Select log file', fpath);
end
params.file.name = fname; % file name only
......@@ -210,10 +232,7 @@ if logAnalysisToggle == 1
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_trim_add_Sum.data;
roll_measured_IMU = dataStruct.Roll_Constant.data;
......@@ -222,9 +241,9 @@ if logAnalysisToggle == 1
IMU_angle_data = timeseries( IMU_angle_arr, time);
% Pull VRPN pitch and roll
pitch_measured_VRPN = dataStruct.VRPN_Pitch_Constant.data;
roll_measured_VRPN = dataStruct.VRPN_Roll_Constant.data;
pitch_measured_VRPN = dataStruct.Pitch_trim_add_Sum.data;
% Define setpoint timeseries
x_setpoint_model = timeseries(x_setpoint, time);
y_setpoint_model = timeseries(y_setpoint, time);
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment