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

Verified new control structure and physics by comparing to Matt's model

parent c582672a
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
controls/model/Library_Blocks/Derivative.slx 0 → 100644
+ 0
− 0
View file @ ffe50504
File added
controls/model/logAnalysis.m
+ 16
− 19
View file @ ffe50504
......@@ -25,7 +25,7 @@ y_command_model_data = y_command_model.signals.values(indices_5ms);
y_position_model_data = y_position_model.signals.values(indices_40ms);
% Pull z control structure data
z_command_model_data = z_command_model.signals.values(indices_5ms);
z_command_model_data = z_command_model.signals.values(indices_40ms);
z_position_model_data = z_position_model.signals.values(indices_40ms);
% Pull yaw control structure data
......@@ -51,7 +51,7 @@ roll_accel_mahony = mahony_reading.signals.values(indices_5ms, 1);
% Plot x position controller output
figure(1); ax1 = subplot(2, 2, 1);
stairs(time, x_vel_raw, '.-'); hold on; grid minor;
stairs(time, x_vel_setpoint, '.-'); hold on; grid minor;
stairs(time_model_40ms, x_vel_setpoint_model_data, '.-'); hold off;
title('X Position Controller Output');
xlabel('Time (s)');
......@@ -91,7 +91,7 @@ linkaxes([ax1, ax2, ax3, ax4], 'x');
% Plot y position controller output
figure(2); ax1 = subplot(2, 2, 1);
stairs(time, y_vel_raw, '.-'); hold on; grid minor;
stairs(time, y_vel_setpoint, '.-'); hold on; grid minor;
stairs(time_model_40ms, y_vel_setpoint_model_data, '.-'); hold off;
title('Y Position Controller Output');
xlabel('Time (s)');
......@@ -133,7 +133,7 @@ linkaxes([ax1, ax2, ax3, ax4], 'x');
% Plot z controller command
figure(3); ax1 = subplot(2, 1, 1);
stairs(time, z_command, '.-'); hold on; grid minor;
stairs(time_model_5ms, z_command_model_data, '.-'); hold off;
stairs(time_model_40ms, z_command_model_data, '.-'); hold off;
title('Z Command');
xlabel('Time (s)');
ylabel('Command');
......@@ -184,7 +184,7 @@ linkaxes([ax1, ax2, ax3], 'x');
%% Plot PWM Commands
figure(5); ax1 = subplot(2, 2, 1);
stairs(time, PWM0,'.-'); hold on; grid minor;
%stairs(time_model_5ms, PWM0_model, '.-'); hold off;
stairs(time_model_5ms, PWM0_model, '.-'); hold off;
title('PWM0 Value');
xlabel('Time (s)');
ylabel('PWM0 Command');
......@@ -192,7 +192,7 @@ legend('Log', 'Model', 'location', 'northwest');
ax2 = subplot(2, 2, 2);
stairs(time, PWM1,'.-'); hold on; grid minor;
%stairs(time_model_5ms, PWM1_model, '.-'); hold off;
stairs(time_model_5ms, PWM1_model, '.-'); hold off;
title('PWM1 Value');
xlabel('Time (s)');
ylabel('PWM1 Command');
......@@ -200,7 +200,7 @@ legend('Log', 'Model', 'location', 'northwest');
ax3 = subplot(2, 2, 3);
stairs(time, PWM2,'.-'); hold on; grid minor;
%stairs(time_model_5ms, PWM2_model, '.-'); hold off;
stairs(time_model_5ms, PWM2_model, '.-'); hold off;
title('PWM2 Value');
xlabel('Time (s)');
ylabel('PWM2 Command');
......@@ -208,7 +208,7 @@ legend('Log', 'Model', 'location', 'northwest');
ax4 = subplot(2, 2, 4);
stairs(time, PWM3,'.-'); hold on; grid minor;
%stairs(time_model_5ms, PWM3_model, '.-'); hold off;
stairs(time_model_5ms, PWM3_model, '.-'); hold off;
title('PWM3 Value');
xlabel('Time (s)');
ylabel('PWM3 Command');
......@@ -219,26 +219,23 @@ linkaxes([ax1, ax2, ax3, ax4], 'xy');
%% Plot output of complimentary filter
figure(8); ax1 = subplot(2, 1, 1);
%stairs(time, pitch_measured_VRPN * (180/pi), '.-'); hold on; grid minor;
stairs(time, (pitch_measured_IMU) * (180/pi), '.-'); hold on; grid minor;
stairs(time_model_5ms, pitch_accel * (180/pi), '.-');
%stairs(time_model_5ms, pitch_accel_mahony * (180/pi), '.-'); hold off;
stairs(time, pitch_measured_VRPN * (180/pi), '.-'); hold on; grid minor;
stairs(time, (pitch_measured_IMU) * (180/pi), '.-');
stairs(time_model_5ms, (pitch_accel + 0.045) * (180/pi), '.-');
title('Pitch Complementary Filter Output');
xlabel('Time (s)');
ylabel('Pitch Angle (degrees)');
%legend('VRPN','IMU', 'Model', 'Mahony', 'location', 'northwest');
legend('IMU', 'Model','location', 'northwest');
legend('VRPN','IMU', 'Model', 'location', 'northwest');
ax2 = subplot(2, 1, 2);
%stairs(time, roll_measured_VRPN * (180/pi), '.-'); hold on; grid minor;
stairs(time, roll_measured_IMU * (180/pi), '.-'); hold on; grid minor;
stairs(time, roll_measured_VRPN * (180/pi), '.-'); hold on; grid minor;
stairs(time, roll_measured_IMU * (180/pi), '.-');
stairs(time_model_5ms, roll_accel * (180/pi), '.-');
%stairs(time_model_5ms, roll_accel_mahony * (180/pi), '.-'); hold off;
title('Roll Complementary Filter Output');
xlabel('Time (s)');
ylabel('Roll Angle (degrees)');
%legend('VRPN','IMU', 'Model', 'Mahony', 'location', 'northwest');
legend('IMU', 'Model','location', 'northwest');
legend('VRPN','IMU', 'Model', 'location', 'northwest');
linkaxes([ax1, ax2], 'x');
......
controls/model/modelParameters.m
+ 144
− 13
View file @ ffe50504
% Add Library Blocks to Path
path = genpath('Library_blocks');
addpath(path);
% Log Analysis Toggle
logAnalysisToggle = 1; % 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 = 20;
runtime = 30;
% Model Parameters
m = 1.216; % Quadrotor + battery mass
......@@ -103,7 +110,7 @@ if logAnalysisToggle == 1
time = dataStruct.Time.data;
time = time - time(1);
time_indices = length(time);
runtime = max(time);
% Determine x position error
......@@ -137,20 +144,20 @@ if logAnalysisToggle == 1
y_vel = timeseries(y_vel_raw, time);
% Determine pitch error
pitch_setpoint = dataStruct.X_pos_PID_Correction.data;
pitch_setpoint = dataStruct.X_Vel_PID_Correction.data;
pitch_angle_raw = dataStruct.Pitch_trim_add_Sum.data;
pitch_error = timeseries(pitch_setpoint - pitch_angle_raw, time);
pitch_angle = timeseries(pitch_angle_raw, time);
% Determine roll error
roll_setpoint = dataStruct.Y_pos_PID_Correction.data;
roll_setpoint = dataStruct.Y_Vel_PID_Correction.data;
roll_angle_raw = dataStruct.Roll_Constant.data;
roll_error = timeseries(roll_setpoint - roll_angle_raw, time);
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_angle_raw = zeros(length(time), 1); % NEEDS UPDATED WHEN LOG FILE INCLUDES YAW VALUE
yaw_angle_raw = dataStruct.Yaw_Constant.data;
yaw_error = timeseries(yaw_setpoint - yaw_angle_raw, time);
yaw_angle = timeseries(yaw_angle_raw, time);
......@@ -205,7 +212,6 @@ if logAnalysisToggle == 1
% Create time series object for z command
throttle_command = timeseries(dataStruct.RC_Throttle_Constant.data, time);
%z_command = dataStruct.RC_Throttle_Constant.data;
% Pull the measurements from the complimentary filter
pitch_measured_IMU = dataStruct.Pitch_trim_add_Sum.data;
......@@ -218,11 +224,11 @@ 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
% % 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
% % 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 * ...
......@@ -231,24 +237,149 @@ if logAnalysisToggle == 1
% omega2_o = omega0_o;
% omega3_o = omega0_o;
%
% Set initial positions
% % Set initial positions
% x_o = x_position(1);
% y_o = y_position(1);
% z_o = z_position(1);
%
% Set initial velocities
% % 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
% % Equilibrium angles
% roll_o = roll_measured_VRPN(1);
% pitch_o = pitch_measured_VRPN(1);
% yaw_o = 0;
%
% Equilibrium angular rates
% % 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;
end
if physicsVerificationToggle == 1
% PHYSICAL SYSTEM PARAMETERS
% quadrotor + battery mass
m = 1.216;
% quadrotor + battery inertia tensor
J = diag([ 0.0130 0.0140 0.0285 ]);
% quadrotor + battery body frame origin to c.o.g. error radius
r_oc = [ 0 ; 0 ; 0 ];
% mass of rigidly attached load
m_L = 0;
% dimensions of assuemd rectangular load
x_L = 0; y_L = 0; z_L = 0;
x_L = 0; y_L = 0; z_L = 0;
% inertia tensor of load
J_L = m_L/12*diag([y_L^2+z_L^2,x_L^2+z_L^2,x_L^2+y_L^2]);
% location of load center of mass in body frame
r_oL = [0; 0; 0];%[ 0.12 ; 0.12 ; -0.04];%(0.04 + z_L/2) ];
%r_oL = [ .115 ; .115 ; 0.04];
% acceleration of gravity
g = 9.81;
% initial dynamics states conditions
u0 = 0;
v0 = 0;
w0 = 0;
p0 = 0;
q0 = 0;
r0 = 0;
x0 = 0;
y0 = 0;
z0 = 0;
phi0 = 0;
theta0 = 0;
psi0 = 0;
% ROTOR PARAMETERS
% nominally equal rotor parameters
K_T = cell(1,4); K_H = cell(1,4); K_d = cell(1,4);
Delta_T = cell(1,4);
%Delta_T = [0 0 0];
J_r = cell(1,4); Gamma_H = cell(1,4);
for i = 1:4
% rotor thrust constants
K_T{i} = 1.2007e-05;
% rotor H-force coefficeint
K_H{i} = 0;
% rotor drag constant
K_d{i} = 1.4852e-07;
% rotor b_z axis velocity thrust adjustment factor
Delta_T{i} = 0;
% 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 ];
end
% ESC-MOTOR PARAMETERS
have_motor_transient = 1;
% nominally equal motor and eletronic speed control parameters
R_m = cell(1,4); K_Q = cell(1,4); K_V = cell(1,4);
i_0 = cell(1,4); omega_0 = cell(1,4); K_E = cell(1,4);
p_0 = cell(1,4); p_max = cell(1,4);
for i = 1:4
% motor resistance
R_m{i} = 0.235;
% motor torque constant
K_Q{i} = 96.3422;
% motor back-emf constant
K_V{i} = 96.3422;
% motor no-load current
i_0{i} = 0.3836;
% initial motor/rotor speed
omega_0{i} = (m*g/4/K_T{1})^0.5;
% ESC normalized pwm input factor
K_E{i} = 1;
% ESC turn-on duty cycle
p_0{i} = 0;
% ESC maximum duty cycle (as calibrated)
p_max{i} = 1;
end
% ROTOR LOCATION/ORIENTATION PARAMETERS
% locatin of hub 1 in body frame
r_h{1} = [ rhx ; rhy ; -rhz ];
% rotor 1 thrust unit vector
Gamma_T{1} = [ 0 ; 0 ; -1 ];
% rotor 1 rotation unit vector
Gamma_Omega{1} = [ 0 ; 0 ; -1 ];
r_h{2} = [ -rhx ; rhy ; -rhz ];
Gamma_T{2} = [ 0 ; 0 ; -1 ];
Gamma_Omega{2} = [ 0 ; 0 ; 1 ];
r_h{3} = [ -rhx ; -rhy ; -rhz ];
Gamma_T{3} = [ 0 ; 0 ; -1 ];
Gamma_Omega{3} = [ 0 ; 0 ; -1 ];
r_h{4} = [ rhx ; -rhy ; -rhz ];
Gamma_T{4} = [ 0 ; 0 ; -1 ];
Gamma_Omega{4} = [ 0 ; 0 ; 1 ];
% BATTERY PARAMETERS
% starting/operating battery voltage
V_0 = 11.4; %12.4; %11.4;
% approximate hovering rate of battery discharge
d_V = 0;
% DISTURBANCE PARAMETERS
F_D = 0;
Q_D = 0;
end
\ No newline at end of file
controls/model/test_model.slx
+ 0
− 0
View file @ ffe50504
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