Merge branch 'master' of https://git.ece.iastate.edu/danc/MicroCART_17-18

controls/model/logAnalysis.m

@@ -131,7 +131,7 @@ linkaxes([ax1, ax2, ax3, ax4], 'x');
 %% Plot z control structure
 
 % Plot z controller command
-figure(3); ax1 = subplot(2, 1, 1);
+figure(3);
 stairs(time, z_command, '.-'); hold on; grid minor;
 stairs(time_model_40ms, z_command_model_data, '.-'); hold off;
 title('Z Command');
@@ -139,21 +139,10 @@ xlabel('Time (s)');
 ylabel('Command');
 legend('Log', 'Model', 'location', 'northwest');
 
-% Plot z position
-ax2 = subplot(2, 1, 2);
-stairs(time, z_pos_raw, '.-'); hold on; grid minor;
-stairs(time_model_40ms, z_position_model_data, '.-'); hold off;
-title('Z Position');
-xlabel('Time (s)');
-ylabel('Position (m)');
-legend('Log', 'Model', 'location', 'northwest');
-
-linkaxes([ax1, ax2], 'x');
-
 %% Plot yaw control structure
 
 % Plot yaw controller output
-figure(4); ax1 = subplot(2, 2, 1);
+figure(4); ax1 = subplot(2, 1, 1);
 stairs(time, yawrate_setpoint,'.-'); hold on; grid minor;
 stairs(time_model_40ms, yawrate_setpoint_model_data, '.-'); hold off;
 title('Yaw Controller Output');
@@ -162,7 +151,7 @@ ylabel('d\psi/dt (rad/s)');
 legend('Log', 'Model', 'location', 'northwest');
 
 % Plot yaw controller command
-ax2 = subplot(2, 2, 2);
+ax2 = subplot(2, 1, 2);
 stairs(time, yaw_command, '.-'); hold on; grid minor;
 stairs(time_model_5ms, yaw_command_model_data, '.-'); hold off;
 title('Yaw Command');
@@ -170,16 +159,7 @@ xlabel('Time (s)');
 ylabel('Command');
 legend('Log', 'Model', 'location', 'northwest');
 
-% Plot yaw position
-ax3 = subplot(2, 2, 3);
-stairs(time, yaw_angle_raw, '.-'); hold on; grid minor;
-stairs(time_model_40ms, yaw_value_model_data, '.-'); hold off;
-title('Yaw Position');
-xlabel('Time (s)');
-ylabel('Value (rad)');
-legend('Log', 'Model', 'location', 'northwest');
-
-linkaxes([ax1, ax2, ax3], 'x');
+linkaxes([ax1, ax2], 'x');
 
 %% Plot PWM Commands
 figure(5); ax1 = subplot(2, 2, 1);
@@ -241,9 +221,9 @@ linkaxes([ax1, ax2], 'x');
 
 %% Plot VRPN Position
 
-figure(9); ax1 = subplot(3, 1, 1);
+figure(10); ax1 = subplot(3, 1, 1);
 stairs(time, x_pos_raw, '.-'); hold on; grid minor;
-%stairs(time_model_40ms, x_position_model_data, '.-');
+stairs(time_model_40ms, x_position_model_data, '.-');
 stairs(time, x_setpoint, '.-');
 title('X position');
 xlabel('Time (s)');
@@ -252,7 +232,7 @@ legend('X position', 'X position model', 'X setpoint');
 
 ax2 = subplot(3, 1, 2);
 stairs(time, y_pos_raw, '.-'); hold on; grid minor;
-%stairs(time_model_40ms, y_position_model_data, '.-');
+stairs(time_model_40ms, y_position_model_data, '.-');
 stairs(time, y_setpoint, '.-');
 title('Y position');
 xlabel('Time (s)');
@@ -262,10 +242,11 @@ legend('Y position', 'Y position model', 'Y setpoint');
 ax3 = subplot(3, 1, 3);
 stairs(time, z_pos_raw, '.-'); hold on; grid minor;
 stairs(time_model_40ms, z_position_model_data, '.-');
+stairs(time, z_setpoint, '.-');
 title('Z position');
 xlabel('Time (s)');
 ylabel('Z position');
-legend('Z position', 'Z position model', 'Y setpoint');
+legend('Z position', 'Z position model', 'Z setpoint');
 
 linkaxes([ax1, ax2, ax3], 'x');
 

controls/model/modelParameters.m

@@ -3,14 +3,14 @@
     addpath(path);
 
 % 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 = 30;
+     runtime = 55;
     
 % Model Parameters
     m = 1.216;                      % Quadrotor + battery mass
@@ -20,7 +20,7 @@
     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 = 0;                         % Rotor in-plane drag 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
@@ -30,17 +30,18 @@
     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
+    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 = 11.4;                      % Nominal battery voltage (V)
+    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 ];
     
     % Define Biquad Filter Parameters
     b0 = 0.020083;
@@ -112,7 +113,7 @@ if logAnalysisToggle == 1
     time_indices = length(time);
     
     runtime = max(time);
- 
+    
     % Determine x position error
     x_setpoint = dataStruct.X_Setpoint_Constant.data;
     x_pos_raw = dataStruct.VRPN_X_Constant.data;
@@ -156,7 +157,7 @@ if logAnalysisToggle == 1
     roll_angle = timeseries(roll_angle_raw, time);
     
     % Determine yaw error
-    yaw_setpoint = zeros(length(time), 1); % NEEDS UPDATED WHEN LOG FILE INCLUDES YAW SETPOINT
+    yaw_setpoint = dataStruct.Yaw_Setpoint_Constant.data;
     yaw_angle_raw = dataStruct.Yaw_Constant.data;
     yaw_error = timeseries(yaw_setpoint - yaw_angle_raw, time);
     yaw_angle = timeseries(yaw_angle_raw, time);
@@ -224,38 +225,10 @@ if logAnalysisToggle == 1
     pitch_measured_VRPN = dataStruct.VRPN_Pitch_Constant.data;
     roll_measured_VRPN = dataStruct.VRPN_Roll_Constant.data;
     
-%     % Set height_controlled_o to initial throttle command
-%     height_controlled_o = dataStruct.RC_Throttle_Constant.data(1);
-%     
-%     % Set rotor speed initial conditions to there calculated values based on
-%     % initial throttle control
-%     omega0_o = (sqrt(((height_controlled_o ...
-%     - Pmin)/(Pmax - Pmin)* Vb - Rm * If) * 4 * ...
-%     Rm * Kv^2 * Kq * Kd + 1) - 1) / (2 * Rm *  ...
-%     Kv * Kq * Kd);
-%     omega1_o = omega0_o;
-%     omega2_o = omega0_o;
-%     omega3_o = omega0_o;
-%     
-%     % Set initial positions
-%     x_o = x_position(1);
-%     y_o = y_position(1);
-%     z_o = z_position(1);
-%     
-%     % Set initial velocities
-%     x_vel_o = (x_position(1) - x_position(2))/(time(1) - time(2));
-%     y_vel_o = (y_position(1) - y_position(2))/(time(1) - time(2));
-%     z_vel_o = (z_position(1) - z_position(2))/(time(1) - time(2));
-%     
-%     % Equilibrium angles
-%     roll_o = roll_measured_VRPN(1);
-%     pitch_o = pitch_measured_VRPN(1);
-%     yaw_o = 0;
-%     
-%     % Equilibrium angular rates
-%     rollrate_o = (roll_measured_VRPN(1) - roll_measured_VRPN(2))/(time(1) - time(2));
-%     pitchrate_o = (pitch_measured_VRPN(1) - pitch_measured_VRPN(2))/(time(1) - time(2));
-%     yawrate_o = 0;
+    % Define setpoint timeseries
+    x_setpoint_model = timeseries(x_setpoint, time);
+    y_setpoint_model = timeseries(y_setpoint, time);
+    z_setpoint_model = timeseries(z_setpoint, time);
     
 end
 
@@ -308,15 +281,15 @@ for i = 1:4
 	% rotor thrust constants
 	K_T{i} = 1.2007e-05; 
 	% rotor H-force coefficeint
-	K_H{i} = 0; 
+	K_H{i} = 3.4574e-04; 
 	% rotor drag constant
 	K_d{i} = 1.4852e-07;
 	% rotor b_z axis velocity thrust adjustment factor 
-	Delta_T{i} = 0; 
+	Delta_T{i} = 0.5*[ 0 0 2.351e-4 ]; 
 	% equivalent rotor + motor moment of inertia 
 	J_r{i} = 4.2012e-05;
     % h-force xy-plane selection matrix
-    Gamma_H{i} = [ 0 0 0 ; 0 0 0 ; 0 0 0 ]; 
+    Gamma_H{i} = [ 1 0 0 ; 0 1 0 ; 0 0 0 ]; 
 end
 
 
@@ -380,6 +353,6 @@ d_V = 0;
 
 % DISTURBANCE PARAMETERS
 
-F_D = 0; 
-Q_D = 0;
+F_D = 1*randn(3,1); 
+Q_D = 0.05*randn(3,1);
 end
\ No newline at end of file