diff --git a/controls/model/logAnalysis.m b/controls/model/logAnalysis.m
index 06511cabce98b8521383109b8eaa3852429f6f23..8875fdb5cd7795e672a071bfd660023edcfd0815 100644
--- a/controls/model/logAnalysis.m
+++ b/controls/model/logAnalysis.m
@@ -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)');
diff --git a/controls/model/modelParameters.m b/controls/model/modelParameters.m
index 3ad190cf93f61fe0b5149b9e8f70b9ab297e75a4..0b7971269fcd3f3bef9de1f2b0e2f69a8a83d203 100644
--- a/controls/model/modelParameters.m
+++ b/controls/model/modelParameters.m
@@ -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;
diff --git a/controls/model/test_model.slx b/controls/model/test_model.slx
index 62dc0a21547c1eff7b1e558e74d80030e2955325..d5bc5cf9bf30f0bb5a79b4167ac79bdb8b1be8a8 100644
Binary files a/controls/model/test_model.slx and b/controls/model/test_model.slx differ