Skip to content
Snippets Groups Projects
Commit e16cd216 authored by Andy Snawerdt's avatar Andy Snawerdt
Browse files

Added in-plane drag friction and transient on thrust to model - this version... 

Added in-plane drag friction and transient on thrust to model - this version seems to be matching the system
parent d60371f2
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
controls/model/logAnalysis.m
+ 9
− 28
View file @ e16cd216
......@@ -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
+ 18
− 45
View file @ e16cd216
......@@ -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
controls/model/test_model.slx
+ 0
− 0
View file @ e16cd216
No preview for this file type
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