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

Updated model to include velocity controller and two degrees freedom... 

Updated model to include velocity controller and two degrees freedom controllers, still contains bug
parent 62d21305
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
controls/model/logAnalysis.m
+ 58
− 50
View file @ 557c25c3
...@@ -11,12 +11,14 @@ time_model_40ms = pitch_setpoint_model.time(indices_40ms); ...@@ -11,12 +11,14 @@ time_model_40ms = pitch_setpoint_model.time(indices_40ms);
time_model_5ms = x_command_model.time(indices_5ms); time_model_5ms = x_command_model.time(indices_5ms);
% Pull x control structure data % Pull x control structure data
x_vel_setpoint_model_data = x_vel_setpoint_model.signals.values(indices_40ms);
pitch_setpoint_model_data = pitch_setpoint_model.signals.values(indices_40ms); pitch_setpoint_model_data = pitch_setpoint_model.signals.values(indices_40ms);
pitchrate_setpoint_model_data = pitchrate_setpoint_model.signals.values(indices_5ms); pitchrate_setpoint_model_data = pitchrate_setpoint_model.signals.values(indices_5ms);
x_command_model_data = x_command_model.signals.values(indices_5ms); x_command_model_data = x_command_model.signals.values(indices_5ms);
x_position_model_data = x_position_model.signals.values(indices_40ms); x_position_model_data = x_position_model.signals.values(indices_40ms);
% Pull y control structure data % Pull y control structure data
y_vel_setpoint_model_data = y_vel_setpoint_model.signals.values(indices_40ms);
roll_setpoint_model_data = roll_setpoint_model.signals.values(indices_40ms); roll_setpoint_model_data = roll_setpoint_model.signals.values(indices_40ms);
rollrate_setpoint_model_data = rollrate_setpoint_model.signals.values(indices_5ms); rollrate_setpoint_model_data = rollrate_setpoint_model.signals.values(indices_5ms);
y_command_model_data = y_command_model.signals.values(indices_5ms); y_command_model_data = y_command_model.signals.values(indices_5ms);
...@@ -38,10 +40,8 @@ PWM2_model = motorCommands.signals.values(indices_5ms, 3); ...@@ -38,10 +40,8 @@ PWM2_model = motorCommands.signals.values(indices_5ms, 3);
PWM3_model = motorCommands.signals.values(indices_5ms, 4); PWM3_model = motorCommands.signals.values(indices_5ms, 4);
% Pull accelerometer readings from model % Pull accelerometer readings from model
pitch_accel = angle_IMU_reading.signals.values(2, 1, indices_5ms); pitch_accel = angle_IMU_reading.signals.values(indices_5ms, 2);
pitch_accel = reshape(pitch_accel, [length(pitch_accel) , 1] ); roll_accel = angle_IMU_reading.signals.values(indices_5ms, 1);
roll_accel = angle_IMU_reading.signals.values(1, 1, indices_5ms);
roll_accel = reshape(roll_accel, [length(roll_accel) , 1] );
% Pull mahony filter data % Pull mahony filter data
pitch_accel_mahony = mahony_reading.signals.values(indices_5ms, 2); pitch_accel_mahony = mahony_reading.signals.values(indices_5ms, 2);
...@@ -49,17 +49,26 @@ roll_accel_mahony = mahony_reading.signals.values(indices_5ms, 1); ...@@ -49,17 +49,26 @@ roll_accel_mahony = mahony_reading.signals.values(indices_5ms, 1);
%% Plot x control structure %% Plot x control structure
% Plot lateral controller output % Plot x position controller output
figure(1); ax1 = subplot(2, 2, 1); figure(1); ax1 = subplot(2, 2, 1);
stairs(time, x_vel_raw, '.-'); hold on; grid minor;
stairs(time_model_40ms, x_vel_setpoint_model_data, '.-'); hold off;
title('X Position Controller Output');
xlabel('Time (s)');
ylabel('Velocity (m/s)');
legend('Log', 'Model', 'location', 'northwest');
% Plot lateral controller output
ax2 = subplot(2, 2, 2);
stairs(time, pitch_setpoint, '.-'); hold on; grid minor; stairs(time, pitch_setpoint, '.-'); hold on; grid minor;
stairs(time_model_40ms, pitch_setpoint_model_data, '.-'); hold off; stairs(time_model_40ms, pitch_setpoint_model_data, '.-'); hold off;
title('Lateral Controller Output'); title('X Velocity Controller Output');
xlabel('Time (s)'); xlabel('Time (s)');
ylabel('\theta (rad)'); ylabel('\theta (rad)');
legend('Log', 'Model', 'location', 'northwest'); legend('Log', 'Model', 'location', 'northwest');
% Plot pitch controller output % Plot pitch controller output
ax2 = subplot(2, 2, 2); ax3 = subplot(2, 2, 3);
stairs(time, pitchrate_setpoint,'.-'); hold on; grid minor; stairs(time, pitchrate_setpoint,'.-'); hold on; grid minor;
stairs(time_model_5ms, pitchrate_setpoint_model_data, '.-'); hold off; stairs(time_model_5ms, pitchrate_setpoint_model_data, '.-'); hold off;
title('Pitch Controller Output'); title('Pitch Controller Output');
...@@ -68,7 +77,7 @@ ylabel('d\theta/dt (rad/s)'); ...@@ -68,7 +77,7 @@ ylabel('d\theta/dt (rad/s)');
legend('Log', 'Model', 'location', 'northwest'); legend('Log', 'Model', 'location', 'northwest');
% Plot x controller command % Plot x controller command
ax3 = subplot(2, 2, 3); ax4 = subplot(2, 2, 4);
stairs(time, x_command, '.-'); hold on; grid minor; stairs(time, x_command, '.-'); hold on; grid minor;
stairs(time_model_5ms, x_command_model_data, '.-'); hold off; stairs(time_model_5ms, x_command_model_data, '.-'); hold off;
title('Pitch Rate Controller Output'); title('Pitch Rate Controller Output');
...@@ -76,30 +85,31 @@ xlabel('Time (s)'); ...@@ -76,30 +85,31 @@ xlabel('Time (s)');
ylabel('Command'); ylabel('Command');
legend('Log', 'Model', 'location', 'northwest'); legend('Log', 'Model', 'location', 'northwest');
% Plot x position
ax4 = subplot(2, 2, 4);
stairs(time, x_position, '.-'); hold on; grid minor;
stairs(time_model_40ms, x_position_model_data, '.-'); hold off;
title('X Position');
xlabel('Time (s)');
ylabel('Position (m)');
legend('Log', 'Model', 'location', 'northwest');
linkaxes([ax1, ax2, ax3, ax4], 'x'); linkaxes([ax1, ax2, ax3, ax4], 'x');
%% Plot y control structure %% Plot y control structure
% Plot longitude controller output % Plot y position controller output
figure(2); ax1 = subplot(2, 2, 1); figure(2); ax1 = subplot(2, 2, 1);
stairs(time, y_vel_raw, '.-'); hold on; grid minor;
stairs(time_model_40ms, y_vel_setpoint_model_data, '.-'); hold off;
title('Y Position Controller Output');
xlabel('Time (s)');
ylabel('Velocity (m/s)');
legend('Log', 'Model', 'location', 'northwest');
% Plot y velocity controller output
ax2 = subplot(2, 2, 2);
stairs(time, roll_setpoint, '.-'); hold on; grid minor; stairs(time, roll_setpoint, '.-'); hold on; grid minor;
stairs(time_model_40ms, roll_setpoint_model_data, '.-'); hold off; stairs(time_model_40ms, roll_setpoint_model_data, '.-'); hold off;
title('Longitude Controller Output '); title('Y Velocity Controller Output ');
xlabel('Time (s)'); xlabel('Time (s)');
ylabel('\phi (rad)'); ylabel('\phi (rad)');
legend('Log', 'Model', 'location', 'northwest'); legend('Log', 'Model', 'location', 'northwest');
% Plot roll controller output % Plot roll controller output
ax2 = subplot(2, 2, 2); ax3 = subplot(2, 2, 3);
stairs(time, rollrate_setpoint,'.-'); hold on; grid minor; stairs(time, rollrate_setpoint,'.-'); hold on; grid minor;
stairs(time_model_5ms, rollrate_setpoint_model_data, '.-'); hold off; stairs(time_model_5ms, rollrate_setpoint_model_data, '.-'); hold off;
title('Roll Controller Output'); title('Roll Controller Output');
...@@ -108,22 +118,13 @@ ylabel('d\phi/dt (rad/s)'); ...@@ -108,22 +118,13 @@ ylabel('d\phi/dt (rad/s)');
legend('Log', 'Model', 'location', 'northwest'); legend('Log', 'Model', 'location', 'northwest');
% Plot y controller command % Plot y controller command
ax3 = subplot(2, 2, 3); ax4 = subplot(2, 2, 4);
stairs(time, y_command, '.-'); hold on; grid minor; stairs(time, y_command, '.-'); hold on; grid minor;
stairs(time_model_5ms, y_command_model_data, '.-'); hold off; stairs(time_model_5ms, y_command_model_data, '.-'); hold off;
title('Y Command'); title('Y Command');
xlabel('Time (s)'); xlabel('Time (s)');
ylabel('Command'); ylabel('Command');
legend('Log', 'Model', 'location', 'northwest'); legend('Log', 'Model', 'location', 'northwest');
% Plot y position
ax4 = subplot(2, 2, 4);
stairs(time, y_position, '.-'); hold on; grid minor;
stairs(time_model_40ms, y_position_model_data, '.-'); hold off;
title('Y Position');
xlabel('Time (s)');
ylabel('Position (m)');
legend('Log', 'Model', 'location', 'northwest');
linkaxes([ax1, ax2, ax3, ax4], 'x'); linkaxes([ax1, ax2, ax3, ax4], 'x');
...@@ -140,7 +141,7 @@ legend('Log', 'Model', 'location', 'northwest'); ...@@ -140,7 +141,7 @@ legend('Log', 'Model', 'location', 'northwest');
% Plot z position % Plot z position
ax2 = subplot(2, 1, 2); ax2 = subplot(2, 1, 2);
stairs(time, z_position, '.-'); hold on; grid minor; stairs(time, z_pos_raw, '.-'); hold on; grid minor;
stairs(time_model_40ms, z_position_model_data, '.-'); hold off; stairs(time_model_40ms, z_position_model_data, '.-'); hold off;
title('Z Position'); title('Z Position');
xlabel('Time (s)'); xlabel('Time (s)');
...@@ -171,7 +172,7 @@ legend('Log', 'Model', 'location', 'northwest'); ...@@ -171,7 +172,7 @@ legend('Log', 'Model', 'location', 'northwest');
% Plot yaw position % Plot yaw position
ax3 = subplot(2, 2, 3); ax3 = subplot(2, 2, 3);
stairs(time, yaw_value, '.-'); hold on; grid minor; stairs(time, yaw_angle_raw, '.-'); hold on; grid minor;
stairs(time_model_40ms, yaw_value_model_data, '.-'); hold off; stairs(time_model_40ms, yaw_value_model_data, '.-'); hold off;
title('Yaw Position'); title('Yaw Position');
xlabel('Time (s)'); xlabel('Time (s)');
...@@ -218,31 +219,33 @@ linkaxes([ax1, ax2, ax3, ax4], 'xy'); ...@@ -218,31 +219,33 @@ linkaxes([ax1, ax2, ax3, ax4], 'xy');
%% Plot output of complimentary filter %% Plot output of complimentary filter
figure(8); ax1 = subplot(2, 1, 1); figure(8); ax1 = subplot(2, 1, 1);
stairs(time, pitch_measured_VRPN * (180/pi), '.-'); hold on; grid minor; %stairs(time, pitch_measured_VRPN * (180/pi), '.-'); hold on; grid minor;
stairs(time, (pitch_measured_IMU + 0.02) * (180/pi), '.-'); 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 * (180/pi), '.-');
stairs(time_model_5ms, pitch_accel_mahony * (180/pi), '.-'); hold off; %stairs(time_model_5ms, pitch_accel_mahony * (180/pi), '.-'); hold off;
title('Pitch Complementary Filter Output'); title('Pitch Complementary Filter Output');
xlabel('Time (s)'); xlabel('Time (s)');
ylabel('Pitch Angle (degrees)'); ylabel('Pitch Angle (degrees)');
legend('VRPN','IMU', 'Model', 'location', 'northwest'); %legend('VRPN','IMU', 'Model', 'Mahony', 'location', 'northwest');
legend('IMU', 'Model','location', 'northwest');
ax2 = subplot(2, 1, 2); ax2 = subplot(2, 1, 2);
stairs(time, roll_measured_VRPN * (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, roll_measured_IMU * (180/pi), '.-'); hold on; grid minor;
%stairs(time_model_5ms, roll_accel * (180/pi), '.-'); stairs(time_model_5ms, roll_accel * (180/pi), '.-');
stairs(time_model_5ms, roll_accel_mahony * (180/pi), '.-'); hold off; %stairs(time_model_5ms, roll_accel_mahony * (180/pi), '.-'); hold off;
title('Roll Complementary Filter Output'); title('Roll Complementary Filter Output');
xlabel('Time (s)'); xlabel('Time (s)');
ylabel('Roll Angle (degrees)'); ylabel('Roll Angle (degrees)');
legend('VRPN','IMU', 'Model', 'location', 'northwest'); %legend('VRPN','IMU', 'Model', 'Mahony', 'location', 'northwest');
legend('IMU', 'Model','location', 'northwest');
linkaxes([ax1, ax2], 'x'); linkaxes([ax1, ax2], 'x');
%% Plot VRPN Position %% Plot VRPN Position
figure(9); ax1 = subplot(3, 1, 1); figure(9); ax1 = subplot(3, 1, 1);
stairs(time, x_position, '.-'); hold on; grid minor; 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, '.-'); stairs(time, x_setpoint, '.-');
title('X position'); title('X position');
...@@ -251,18 +254,17 @@ ylabel('X position'); ...@@ -251,18 +254,17 @@ ylabel('X position');
legend('X position', 'X position model', 'X setpoint'); legend('X position', 'X position model', 'X setpoint');
ax2 = subplot(3, 1, 2); ax2 = subplot(3, 1, 2);
stairs(time, y_position, '.-'); hold on; grid minor; 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, x_setpoint, '.-'); stairs(time, y_setpoint, '.-');
title('Y position'); title('Y position');
xlabel('Time (s)'); xlabel('Time (s)');
ylabel('Y position'); ylabel('Y position');
legend('Y position', 'Y position model', 'Y setpoint'); legend('Y position', 'Y position model', 'Y setpoint');
ax3 = subplot(3, 1, 3); ax3 = subplot(3, 1, 3);
stairs(time, y_position, '.-'); hold on; grid minor; stairs(time, z_pos_raw, '.-'); hold on; grid minor;
stairs(time_model_40ms, y_position_model_data, '.-'); stairs(time_model_40ms, z_position_model_data, '.-');
title('Z position'); title('Z position');
xlabel('Time (s)'); xlabel('Time (s)');
ylabel('Z position'); ylabel('Z position');
...@@ -273,18 +275,21 @@ linkaxes([ax1, ax2, ax3], 'x'); ...@@ -273,18 +275,21 @@ linkaxes([ax1, ax2, ax3], 'x');
%% Plot Gyro Data %% Plot Gyro Data
figure(10); ax1 = subplot(3, 1, 1); figure(10); ax1 = subplot(3, 1, 1);
stairs(time, raw_gyro_data_x, '.-'); hold on; grid minor; stairs(time, raw_gyro_data_x, '.-'); hold on; grid minor;
stairs(time_model_5ms, gyro_data_x, '.-');
title('gyro x'); title('gyro x');
xlabel('Time (s)'); xlabel('Time (s)');
ylabel('p (rad/s)'); ylabel('p (rad/s)');
ax2 = subplot(3, 1, 2); ax2 = subplot(3, 1, 2);
stairs(time, raw_gyro_data_y, '.-'); hold on; grid minor; stairs(time, raw_gyro_data_y, '.-'); hold on; grid minor;
stairs(time_model_5ms, gyro_data_y, '.-');
title('gyro y'); title('gyro y');
xlabel('Time (s)'); xlabel('Time (s)');
ylabel('q (rad/s)'); ylabel('q (rad/s)');
ax3 = subplot(3, 1, 3); ax3 = subplot(3, 1, 3);
stairs(time, raw_gyro_data_z, '.-'); hold on; grid minor; stairs(time, raw_gyro_data_z, '.-'); hold on; grid minor;
stairs(time_model_5ms, gyro_data_z, '.-');
title('gyro z'); title('gyro z');
xlabel('Time (s)'); xlabel('Time (s)');
ylabel('r (rad/s)'); ylabel('r (rad/s)');
...@@ -294,18 +299,21 @@ linkaxes([ax1, ax2, ax3], 'x'); ...@@ -294,18 +299,21 @@ linkaxes([ax1, ax2, ax3], 'x');
%% Plot Accel Data %% Plot Accel Data
figure(11); ax1 = subplot(3, 1, 1); figure(11); ax1 = subplot(3, 1, 1);
stairs(time, raw_accel_data_x, '.-'); hold on; grid minor; stairs(time, raw_accel_data_x, '.-'); hold on; grid minor;
stairs(time_model_5ms, accel_data_x, '.-');
title('accel x'); title('accel x');
xlabel('Time (s)'); xlabel('Time (s)');
ylabel('accel x (g)'); ylabel('accel x (g)');
ax2 = subplot(3, 1, 2); ax2 = subplot(3, 1, 2);
stairs(time, raw_gyro_data_y, '.-'); hold on; grid minor; stairs(time, raw_accel_data_y, '.-'); hold on; grid minor;
stairs(time_model_5ms, accel_data_y, '.-');
title('accel y'); title('accel y');
xlabel('Time (s)'); xlabel('Time (s)');
ylabel('accel y (g)'); ylabel('accel y (g)');
ax3 = subplot(3, 1, 3); ax3 = subplot(3, 1, 3);
stairs(time, raw_gyro_data_z, '.-'); hold on; grid minor; stairs(time, raw_accel_data_z, '.-'); hold on; grid minor;
stairs(time_model_5ms, accel_data_z, '.-');
title('accel z'); title('accel z');
xlabel('Time (s)'); xlabel('Time (s)');
ylabel('accel (z)'); ylabel('accel (z)');
......
controls/model/modelParameters.m
+ 66
− 39
View file @ 557c25c3
...@@ -6,7 +6,7 @@ ...@@ -6,7 +6,7 @@
runtime = 20; runtime = 20;
% Model Parameters % Model Parameters
m = 1.140; % Quadrotor + battery mass m = 1.216; % Quadrotor + battery mass
g = 9.81; % Acceleration of gravity g = 9.81; % Acceleration of gravity
Jxx = 0.0130; % Quadrotor and battery motor of inertia around bx (pitch) Jxx = 0.0130; % Quadrotor and battery motor of inertia around bx (pitch)
Jyy = 0.0140; % Quadrotor and battery motor of inertia around by (roll) Jyy = 0.0140; % Quadrotor and battery motor of inertia around by (roll)
...@@ -19,12 +19,12 @@ ...@@ -19,12 +19,12 @@
rhy = 0.16; % Y-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 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 r_oc = [0; 0; 0]; % Vector from origin to center of mass
Rm = 0.2308; % Motor resistance Rm = 0.235; % Motor resistance
Kq = 96.3422; % Motor torque constant Kq = 96.3422; % Motor torque constant
Kv = 96.3422; % Motor back emf constant Kv = 96.3422; % Motor back emf constant
If = 0.3836; % Motor internal friction current If = 0.3836; % Motor internal friction current
Pmin = 1e5; % Minimum zybo output duty cycle command Pmin = 0; % Minimum zybo output duty cycle command
Pmax = 2e5; % Maximum zybo output duty cycle command Pmax = 1; % Maximum zybo output duty cycle command
Tc = 0.04; % Camera system sampling period Tc = 0.04; % Camera system sampling period
Tq = 0.005; % Quad sampling period Tq = 0.005; % Quad sampling period
tau_c = 0; % Camera system total latency tau_c = 0; % Camera system total latency
...@@ -32,9 +32,18 @@ ...@@ -32,9 +32,18 @@
x_controlled_o = 0; % Equilibrium lateral controller output x_controlled_o = 0; % Equilibrium lateral controller output
y_controlled_o = 0; % Equilibrium longitudinal controller output y_controlled_o = 0; % Equilibrium longitudinal controller output
yaw_controlled_o = 0; % Equilibrium yaw controller output yaw_controlled_o = 0; % Equilibrium yaw controller output
Kp_mahony = 0.2; % Proportional term for mahony filter Kp_mahony = 0.4; % Proportional term for mahony filter
Ki_mahony = 0.001; % Integral 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];
% Determine Initial Conditions % Determine Initial Conditions
% Equilibrium rotor speeds % Equilibrium rotor speeds
...@@ -94,56 +103,74 @@ if logAnalysisToggle == 1 ...@@ -94,56 +103,74 @@ if logAnalysisToggle == 1
time = dataStruct.Time.data; time = dataStruct.Time.data;
time = time - time(1); time = time - time(1);
time_indices = length(time); time_indices = length(time);
% time = 0:0.005001:max(time);
% time = time(1:time_indices);
runtime = max(time); runtime = max(time);
% Determine x position error % Determine x position error
x_setpoint = dataStruct.X_Setpoint_Constant.data; x_setpoint = dataStruct.X_Setpoint_Constant.data;
x_position = dataStruct.VRPN_X_Constant.data; x_pos_raw = dataStruct.VRPN_X_Constant.data;
x_error = timeseries(x_setpoint - x_position, time); x_error = timeseries(x_setpoint - x_pos_raw, time);
x_position = timeseries(x_pos_raw, time);
% Determine y position error % Determine y position error
y_setpoint = dataStruct.Y_Setpoint_Constant.data; y_setpoint = dataStruct.Y_Setpoint_Constant.data;
y_position = dataStruct.VRPN_Y_Constant.data; y_pos_raw = dataStruct.VRPN_Y_Constant.data;
y_error = timeseries(y_setpoint - y_position, time); y_error = timeseries(y_setpoint - y_pos_raw, time);
y_position = timeseries(y_pos_raw, time);
% Determine z position error % Determine z position error
z_setpoint = dataStruct.Alt_Setpoint_Constant.data; z_setpoint = dataStruct.Alt_Setpoint_Constant.data;
z_position = dataStruct.VRPN_Alt_Constant.data; z_pos_raw = dataStruct.VRPN_Alt_Constant.data;
z_error = timeseries(z_setpoint - z_position, time); z_error = timeseries(z_setpoint - z_pos_raw, time);
z_position = timeseries(z_pos_raw, time);
% Determine x velocity error
x_vel_setpoint = dataStruct.X_pos_PID_Correction.data;
x_vel_raw = dataStruct.X_Vel_Correction.data;
x_vel_error = timeseries(x_vel_setpoint - x_vel_raw, time);
x_vel = timeseries(x_vel_raw, time);
% Determine y velocity error
y_vel_setpoint = dataStruct.Y_pos_PID_Correction.data;
y_vel_raw = dataStruct.Y_Vel_Correction.data;
y_vel_error = timeseries(y_vel_setpoint - y_vel_raw, time);
y_vel = timeseries(y_vel_raw, time);
% Determine pitch error % Determine pitch error
pitch_setpoint = dataStruct.X_pos_PID_Correction.data; pitch_setpoint = dataStruct.X_pos_PID_Correction.data;
pitch_value = dataStruct.Pitch_trim_add_Sum.data; pitch_angle_raw = dataStruct.Pitch_trim_add_Sum.data;
pitch_error = timeseries(pitch_setpoint - pitch_value, time); pitch_error = timeseries(pitch_setpoint - pitch_angle_raw, time);
pitch_angle = timeseries(pitch_angle_raw, time);
% Determine roll error % Determine roll error
roll_setpoint = dataStruct.Y_pos_PID_Correction.data; roll_setpoint = dataStruct.Y_pos_PID_Correction.data;
roll_value = dataStruct.Roll_Constant.data; roll_angle_raw = dataStruct.Roll_Constant.data;
roll_error = timeseries(roll_setpoint - roll_value, time); roll_error = timeseries(roll_setpoint - roll_angle_raw, time);
roll_angle = timeseries(roll_angle_raw, time);
% Determine yaw error % Determine yaw error
yaw_setpoint = zeros(length(time), 1); % NEEDS UPDATED WHEN LOG FILE INCLUDES YAW SETPOINT yaw_setpoint = zeros(length(time), 1); % NEEDS UPDATED WHEN LOG FILE INCLUDES YAW SETPOINT
yaw_value = zeros(length(time), 1); % NEEDS UPDATED WHEN LOG FILE INCLUDES YAW VALUE yaw_angle_raw = zeros(length(time), 1); % NEEDS UPDATED WHEN LOG FILE INCLUDES YAW VALUE
yaw_error = timeseries(yaw_setpoint - yaw_value, time); yaw_error = timeseries(yaw_setpoint - yaw_angle_raw, time);
yaw_angle = timeseries(yaw_angle_raw, time);
% Determine pitch rate error % Determine pitch rate error
pitchrate_setpoint = dataStruct.Pitch_PID_Correction.data; pitchrate_setpoint = dataStruct.Pitch_PID_Correction.data;
pitchrate_value = dataStruct.gyro_y.data; pitchrate_raw = dataStruct.gyro_y.data;
pitchrate_error = timeseries(pitchrate_setpoint - pitchrate_value, time); pitchrate_error = timeseries(pitchrate_setpoint - pitchrate_raw, time);
pitchrate = timeseries(pitchrate_raw, time);
% Determine roll rate error % Determine roll rate error
rollrate_setpoint = dataStruct.Roll_PID_Correction.data; rollrate_setpoint = dataStruct.Roll_PID_Correction.data;
rollrate_value = dataStruct.gyro_x.data; rollrate_raw = dataStruct.gyro_x.data;
rollrate_error = timeseries(rollrate_setpoint - rollrate_value, time); rollrate_error = timeseries(rollrate_setpoint - rollrate_raw, time);
rollrate = timeseries(rollrate_raw, time);
% Determine yaw rate error % Determine yaw rate error
yawrate_setpoint = dataStruct.Yaw_PID_Correction.data; yawrate_setpoint = dataStruct.Yaw_PID_Correction.data;
yawrate_value = dataStruct.gyro_z.data; yawrate_raw = dataStruct.gyro_z.data;
yawrate_error = timeseries(yawrate_setpoint - yawrate_value, time); yawrate_error = timeseries(yawrate_setpoint - yawrate_raw, time);
yawrate = timeseries(yawrate_raw, time);
% Pull motor commands from log % Pull motor commands from log
x_command = dataStruct.Pitch_Rate_PID_Correction.data; x_command = dataStruct.Pitch_Rate_PID_Correction.data;
...@@ -152,10 +179,10 @@ if logAnalysisToggle == 1 ...@@ -152,10 +179,10 @@ if logAnalysisToggle == 1
yaw_command = dataStruct.Yaw_Rate_PID_Correction.data; yaw_command = dataStruct.Yaw_Rate_PID_Correction.data;
% Determine signal mix PWM values % Determine signal mix PWM values
PWM0 = dataStruct.Signal_Mixer_PWM_0.data; PWM0 = dataStruct.Signal_Mixer_MOTOR_0.data;
PWM1 = dataStruct.Signal_Mixer_PWM_1.data; PWM1 = dataStruct.Signal_Mixer_MOTOR_1.data;
PWM2 = dataStruct.Signal_Mixer_PWM_2.data; PWM2 = dataStruct.Signal_Mixer_MOTOR_2.data;
PWM3 = dataStruct.Signal_Mixer_PWM_3.data; PWM3 = dataStruct.Signal_Mixer_MOTOR_3.data;
PWM_arr = ... PWM_arr = ...
[PWM0, PWM1, PWM2, PWM3]; [PWM0, PWM1, PWM2, PWM3];
motor_commands = timeseries(PWM_arr, time); motor_commands = timeseries(PWM_arr, time);
...@@ -191,11 +218,11 @@ if logAnalysisToggle == 1 ...@@ -191,11 +218,11 @@ if logAnalysisToggle == 1
pitch_measured_VRPN = dataStruct.VRPN_Pitch_Constant.data; pitch_measured_VRPN = dataStruct.VRPN_Pitch_Constant.data;
roll_measured_VRPN = dataStruct.VRPN_Roll_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); % height_controlled_o = dataStruct.RC_Throttle_Constant.data(1);
% %
% % Set rotor speed initial conditions to there calculated values based on % Set rotor speed initial conditions to there calculated values based on
% % initial throttle control % initial throttle control
% omega0_o = (sqrt(((height_controlled_o ... % omega0_o = (sqrt(((height_controlled_o ...
% - Pmin)/(Pmax - Pmin)* Vb - Rm * If) * 4 * ... % - Pmin)/(Pmax - Pmin)* Vb - Rm * If) * 4 * ...
% Rm * Kv^2 * Kq * Kd + 1) - 1) / (2 * Rm * ... % Rm * Kv^2 * Kq * Kd + 1) - 1) / (2 * Rm * ...
...@@ -204,22 +231,22 @@ if logAnalysisToggle == 1 ...@@ -204,22 +231,22 @@ if logAnalysisToggle == 1
% omega2_o = omega0_o; % omega2_o = omega0_o;
% omega3_o = omega0_o; % omega3_o = omega0_o;
% %
% % Set initial positions % Set initial positions
% x_o = x_position(1); % x_o = x_position(1);
% y_o = y_position(1); % y_o = y_position(1);
% z_o = z_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)); % 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)); % 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)); % z_vel_o = (z_position(1) - z_position(2))/(time(1) - time(2));
% %
% % Equilibrium angles % Equilibrium angles
% roll_o = roll_measured_VRPN(1); % roll_o = roll_measured_VRPN(1);
% pitch_o = pitch_measured_VRPN(1); % pitch_o = pitch_measured_VRPN(1);
% yaw_o = 0; % yaw_o = 0;
% %
% % Equilibrium angular rates % Equilibrium angular rates
% rollrate_o = (roll_measured_VRPN(1) - roll_measured_VRPN(2))/(time(1) - time(2)); % 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)); % pitchrate_o = (pitch_measured_VRPN(1) - pitch_measured_VRPN(2))/(time(1) - time(2));
% yawrate_o = 0; % yawrate_o = 0;
......
controls/model/test_model.slx
+ 0
− 0
View file @ 557c25c3
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