diff --git a/controls/model/Quadcopter_Model_R2016_B.slx b/controls/model/Quadcopter_Model_R2016_B.slx
index 5845d44124f35d4a571cc0f9319c8dc54f693804..9ddda3cdbd05db65d455a676fe14b5b871b4b78c 100644
Binary files a/controls/model/Quadcopter_Model_R2016_B.slx and b/controls/model/Quadcopter_Model_R2016_B.slx differ
diff --git a/controls/model/modelParameters.m b/controls/model/modelParameters.m
index beb6434884bf008200fd53dbb289c4656bc4991d..afe8eb6e624c655a1b3a0336d1b87ce3f92c560d 100644
--- a/controls/model/modelParameters.m
+++ b/controls/model/modelParameters.m
@@ -1,91 +1,113 @@
-% 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);