Skip to content
Snippets Groups Projects
Commit c582672a 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 5521309d
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 @ c582672a
......@@ -11,12 +11,14 @@ time_model_40ms = pitch_setpoint_model.time(indices_40ms);
time_model_5ms = x_command_model.time(indices_5ms);
% 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);
pitchrate_setpoint_model_data = pitchrate_setpoint_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);
% 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);
rollrate_setpoint_model_data = rollrate_setpoint_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);
PWM3_model = motorCommands.signals.values(indices_5ms, 4);
% Pull accelerometer readings from model
pitch_accel = angle_IMU_reading.signals.values(2, 1, indices_5ms);
pitch_accel = reshape(pitch_accel, [length(pitch_accel) , 1] );
roll_accel = angle_IMU_reading.signals.values(1, 1, indices_5ms);
roll_accel = reshape(roll_accel, [length(roll_accel) , 1] );
pitch_accel = angle_IMU_reading.signals.values(indices_5ms, 2);
roll_accel = angle_IMU_reading.signals.values(indices_5ms, 1);
% Pull mahony filter data
pitch_accel_mahony = mahony_reading.signals.values(indices_5ms, 2);
......@@ -49,17 +49,26 @@ roll_accel_mahony = mahony_reading.signals.values(indices_5ms, 1);
%% Plot x control structure
% Plot lateral controller output
% Plot x position controller output
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_model_40ms, pitch_setpoint_model_data, '.-'); hold off;
title('Lateral Controller Output');
title('X Velocity Controller Output');
xlabel('Time (s)');
ylabel('\theta (rad)');
legend('Log', 'Model', 'location', 'northwest');
% Plot pitch controller output
ax2 = subplot(2, 2, 2);
ax3 = subplot(2, 2, 3);
stairs(time, pitchrate_setpoint,'.-'); hold on; grid minor;
stairs(time_model_5ms, pitchrate_setpoint_model_data, '.-'); hold off;
title('Pitch Controller Output');
......@@ -68,7 +77,7 @@ ylabel('d\theta/dt (rad/s)');
legend('Log', 'Model', 'location', 'northwest');
% Plot x controller command
ax3 = subplot(2, 2, 3);
ax4 = subplot(2, 2, 4);
stairs(time, x_command, '.-'); hold on; grid minor;
stairs(time_model_5ms, x_command_model_data, '.-'); hold off;
title('Pitch Rate Controller Output');
......@@ -76,30 +85,31 @@ xlabel('Time (s)');
ylabel('Command');
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');
%% Plot y control structure
% Plot longitude controller output
% Plot y position controller output
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_model_40ms, roll_setpoint_model_data, '.-'); hold off;
title('Longitude Controller Output ');
title('Y Velocity Controller Output ');
xlabel('Time (s)');
ylabel('\phi (rad)');
legend('Log', 'Model', 'location', 'northwest');
% Plot roll controller output
ax2 = subplot(2, 2, 2);
ax3 = subplot(2, 2, 3);
stairs(time, rollrate_setpoint,'.-'); hold on; grid minor;
stairs(time_model_5ms, rollrate_setpoint_model_data, '.-'); hold off;
title('Roll Controller Output');
......@@ -108,22 +118,13 @@ ylabel('d\phi/dt (rad/s)');
legend('Log', 'Model', 'location', 'northwest');
% Plot y controller command
ax3 = subplot(2, 2, 3);
ax4 = subplot(2, 2, 4);
stairs(time, y_command, '.-'); hold on; grid minor;
stairs(time_model_5ms, y_command_model_data, '.-'); hold off;
title('Y Command');
xlabel('Time (s)');
ylabel('Command');
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');
......@@ -140,7 +141,7 @@ legend('Log', 'Model', 'location', 'northwest');
% Plot z position
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;
title('Z Position');
xlabel('Time (s)');
......@@ -171,7 +172,7 @@ legend('Log', 'Model', 'location', 'northwest');
% Plot yaw position
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;
title('Yaw Position');
xlabel('Time (s)');
......@@ -218,31 +219,33 @@ 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 + 0.02) * (180/pi), '.-');
%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), '.-'); hold on; grid minor;
stairs(time_model_5ms, pitch_accel * (180/pi), '.-');
%stairs(time_model_5ms, pitch_accel_mahony * (180/pi), '.-'); hold off;
title('Pitch Complementary Filter Output');
xlabel('Time (s)');
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);
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;
%stairs(time, roll_measured_VRPN * (180/pi), '.-'); hold on; grid minor;
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_mahony * (180/pi), '.-'); hold off;
title('Roll Complementary Filter Output');
xlabel('Time (s)');
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');
%% Plot VRPN Position
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, x_setpoint, '.-');
title('X position');
......@@ -251,18 +254,17 @@ ylabel('X position');
legend('X position', 'X position model', 'X setpoint');
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, x_setpoint, '.-');
stairs(time, y_setpoint, '.-');
title('Y position');
xlabel('Time (s)');
ylabel('Y position');
legend('Y position', 'Y position model', 'Y setpoint');
ax3 = subplot(3, 1, 3);
stairs(time, y_position, '.-'); hold on; grid minor;
stairs(time_model_40ms, y_position_model_data, '.-');
stairs(time, z_pos_raw, '.-'); hold on; grid minor;
stairs(time_model_40ms, z_position_model_data, '.-');
title('Z position');
xlabel('Time (s)');
ylabel('Z position');
......@@ -273,18 +275,21 @@ linkaxes([ax1, ax2, ax3], 'x');
%% Plot Gyro Data
figure(10); ax1 = subplot(3, 1, 1);
stairs(time, raw_gyro_data_x, '.-'); hold on; grid minor;
stairs(time_model_5ms, gyro_data_x, '.-');
title('gyro x');
xlabel('Time (s)');
ylabel('p (rad/s)');
ax2 = subplot(3, 1, 2);
stairs(time, raw_gyro_data_y, '.-'); hold on; grid minor;
stairs(time_model_5ms, gyro_data_y, '.-');
title('gyro y');
xlabel('Time (s)');
ylabel('q (rad/s)');
ax3 = subplot(3, 1, 3);
stairs(time, raw_gyro_data_z, '.-'); hold on; grid minor;
stairs(time_model_5ms, gyro_data_z, '.-');
title('gyro z');
xlabel('Time (s)');
ylabel('r (rad/s)');
......@@ -294,18 +299,21 @@ linkaxes([ax1, ax2, ax3], 'x');
%% Plot Accel Data
figure(11); ax1 = subplot(3, 1, 1);
stairs(time, raw_accel_data_x, '.-'); hold on; grid minor;
stairs(time_model_5ms, accel_data_x, '.-');
title('accel x');
xlabel('Time (s)');
ylabel('accel x (g)');
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');
xlabel('Time (s)');
ylabel('accel y (g)');
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');
xlabel('Time (s)');
ylabel('accel (z)');
......
controls/model/modelParameters.m
+ 66
− 39
View file @ c582672a
......@@ -6,7 +6,7 @@
runtime = 20;
% Model Parameters
m = 1.140; % Quadrotor + battery mass
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)
......@@ -19,12 +19,12 @@
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.2308; % Motor resistance
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 = 1e5; % Minimum zybo output duty cycle command
Pmax = 2e5; % 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
......@@ -32,9 +32,18 @@
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.2; % Proportional term for mahony filter
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];
% Determine Initial Conditions
% Equilibrium rotor speeds
......@@ -94,56 +103,74 @@ if logAnalysisToggle == 1
time = dataStruct.Time.data;
time = time - time(1);
time_indices = length(time);
% time = 0:0.005001:max(time);
% time = time(1:time_indices);
runtime = max(time);
% Determine x position error
x_setpoint = dataStruct.X_Setpoint_Constant.data;
x_position = dataStruct.VRPN_X_Constant.data;
x_error = timeseries(x_setpoint - x_position, time);
x_pos_raw = dataStruct.VRPN_X_Constant.data;
x_error = timeseries(x_setpoint - x_pos_raw, time);
x_position = timeseries(x_pos_raw, time);
% Determine y position error
y_setpoint = dataStruct.Y_Setpoint_Constant.data;
y_position = dataStruct.VRPN_Y_Constant.data;
y_error = timeseries(y_setpoint - y_position, time);
y_pos_raw = dataStruct.VRPN_Y_Constant.data;
y_error = timeseries(y_setpoint - y_pos_raw, time);
y_position = timeseries(y_pos_raw, time);
% Determine z position error
z_setpoint = dataStruct.Alt_Setpoint_Constant.data;
z_position = dataStruct.VRPN_Alt_Constant.data;
z_error = timeseries(z_setpoint - z_position, time);
z_pos_raw = dataStruct.VRPN_Alt_Constant.data;
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
pitch_setpoint = dataStruct.X_pos_PID_Correction.data;
pitch_value = dataStruct.Pitch_trim_add_Sum.data;
pitch_error = timeseries(pitch_setpoint - pitch_value, time);
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_value = dataStruct.Roll_Constant.data;
roll_error = timeseries(roll_setpoint - roll_value, time);
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_value = zeros(length(time), 1); % NEEDS UPDATED WHEN LOG FILE INCLUDES YAW VALUE
yaw_error = timeseries(yaw_setpoint - yaw_value, time);
yaw_angle_raw = zeros(length(time), 1); % NEEDS UPDATED WHEN LOG FILE INCLUDES YAW VALUE
yaw_error = timeseries(yaw_setpoint - yaw_angle_raw, time);
yaw_angle = timeseries(yaw_angle_raw, time);
% Determine pitch rate error
pitchrate_setpoint = dataStruct.Pitch_PID_Correction.data;
pitchrate_value = dataStruct.gyro_y.data;
pitchrate_error = timeseries(pitchrate_setpoint - pitchrate_value, time);
pitchrate_raw = dataStruct.gyro_y.data;
pitchrate_error = timeseries(pitchrate_setpoint - pitchrate_raw, time);
pitchrate = timeseries(pitchrate_raw, time);
% Determine roll rate error
rollrate_setpoint = dataStruct.Roll_PID_Correction.data;
rollrate_value = dataStruct.gyro_x.data;
rollrate_error = timeseries(rollrate_setpoint - rollrate_value, time);
rollrate_raw = dataStruct.gyro_x.data;
rollrate_error = timeseries(rollrate_setpoint - rollrate_raw, time);
rollrate = timeseries(rollrate_raw, time);
% Determine yaw rate error
yawrate_setpoint = dataStruct.Yaw_PID_Correction.data;
yawrate_value = dataStruct.gyro_z.data;
yawrate_error = timeseries(yawrate_setpoint - yawrate_value, time);
yawrate_raw = dataStruct.gyro_z.data;
yawrate_error = timeseries(yawrate_setpoint - yawrate_raw, time);
yawrate = timeseries(yawrate_raw, time);
% Pull motor commands from log
x_command = dataStruct.Pitch_Rate_PID_Correction.data;
......@@ -152,10 +179,10 @@ if logAnalysisToggle == 1
yaw_command = dataStruct.Yaw_Rate_PID_Correction.data;
% Determine signal mix PWM values
PWM0 = dataStruct.Signal_Mixer_PWM_0.data;
PWM1 = dataStruct.Signal_Mixer_PWM_1.data;
PWM2 = dataStruct.Signal_Mixer_PWM_2.data;
PWM3 = dataStruct.Signal_Mixer_PWM_3.data;
PWM0 = dataStruct.Signal_Mixer_MOTOR_0.data;
PWM1 = dataStruct.Signal_Mixer_MOTOR_1.data;
PWM2 = dataStruct.Signal_Mixer_MOTOR_2.data;
PWM3 = dataStruct.Signal_Mixer_MOTOR_3.data;
PWM_arr = ...
[PWM0, PWM1, PWM2, PWM3];
motor_commands = timeseries(PWM_arr, time);
......@@ -191,11 +218,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 * ...
......@@ -204,22 +231,22 @@ 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;
......
controls/model/test_model.slx
+ 0
− 0
View file @ c582672a
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