diff --git a/controls/model/logAnalysis.m b/controls/model/logAnalysis.m
index c4f3a1f365deb000fe399bc2a851bd16989a20ff..06511cabce98b8521383109b8eaa3852429f6f23 100644
--- a/controls/model/logAnalysis.m
+++ b/controls/model/logAnalysis.m
@@ -37,16 +37,20 @@ PWM1_model = motorCommands.signals.values(indices_5ms, 2);
PWM2_model = motorCommands.signals.values(indices_5ms, 3);
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 = 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] );
+% Pull mahony filter data
+pitch_accel_mahony = mahony_reading.signals.values(indices_5ms, 2);
+roll_accel_mahony = mahony_reading.signals.values(indices_5ms, 1);
+
%% Plot x control structure
% Plot lateral controller output
-figure(1); subplot(2, 2, 1);
+figure(1); ax1 = subplot(2, 2, 1);
stairs(time, pitch_setpoint, '.-'); hold on; grid minor;
stairs(time_model_40ms, pitch_setpoint_model_data, '.-'); hold off;
title('Lateral Controller Output');
@@ -55,7 +59,7 @@ ylabel('\theta (rad)');
legend('Log', 'Model', 'location', 'northwest');
% Plot pitch controller output
-subplot(2, 2, 2);
+ax2 = subplot(2, 2, 2);
stairs(time, pitchrate_setpoint,'.-'); hold on; grid minor;
stairs(time_model_5ms, pitchrate_setpoint_model_data, '.-'); hold off;
title('Pitch Controller Output');
@@ -64,16 +68,16 @@ ylabel('d\theta/dt (rad/s)');
legend('Log', 'Model', 'location', 'northwest');
% Plot x controller command
-subplot(2, 2, 3);
+ax3 = subplot(2, 2, 3);
stairs(time, x_command, '.-'); hold on; grid minor;
stairs(time_model_5ms, x_command_model_data, '.-'); hold off;
-title('X Command');
+title('Pitch Rate Controller Output');
xlabel('Time (s)');
ylabel('Command');
legend('Log', 'Model', 'location', 'northwest');
% Plot x position
-subplot(2, 2, 4);
+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');
@@ -81,10 +85,12 @@ 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
-figure(2); subplot(2, 2, 1);
+figure(2); ax1 = subplot(2, 2, 1);
stairs(time, roll_setpoint, '.-'); hold on; grid minor;
stairs(time_model_40ms, roll_setpoint_model_data, '.-'); hold off;
title('Longitude Controller Output ');
@@ -93,7 +99,7 @@ ylabel('\phi (rad)');
legend('Log', 'Model', 'location', 'northwest');
% Plot roll controller output
-subplot(2, 2, 2);
+ax2 = subplot(2, 2, 2);
stairs(time, rollrate_setpoint,'.-'); hold on; grid minor;
stairs(time_model_5ms, rollrate_setpoint_model_data, '.-'); hold off;
title('Roll Controller Output');
@@ -102,7 +108,7 @@ ylabel('d\phi/dt (rad/s)');
legend('Log', 'Model', 'location', 'northwest');
% Plot y controller command
-subplot(2, 2, 3);
+ax3 = subplot(2, 2, 3);
stairs(time, y_command, '.-'); hold on; grid minor;
stairs(time_model_5ms, y_command_model_data, '.-'); hold off;
title('Y Command');
@@ -111,7 +117,7 @@ ylabel('Command');
legend('Log', 'Model', 'location', 'northwest');
% Plot y position
-subplot(2, 2, 4);
+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');
@@ -119,10 +125,12 @@ xlabel('Time (s)');
ylabel('Position (m)');
legend('Log', 'Model', 'location', 'northwest');
+linkaxes([ax1, ax2, ax3, ax4], 'x');
+
%% Plot z control structure
% Plot z controller command
-figure(3); subplot(2, 1, 1);
+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;
title('Z Command');
@@ -131,7 +139,7 @@ ylabel('Command');
legend('Log', 'Model', 'location', 'northwest');
% Plot z position
-subplot(2, 1, 2);
+ax2 = subplot(2, 1, 2);
stairs(time, z_position, '.-'); hold on; grid minor;
stairs(time_model_40ms, z_position_model_data, '.-'); hold off;
title('Z Position');
@@ -139,10 +147,12 @@ 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); subplot(2, 2, 1);
+figure(4); ax1 = subplot(2, 2, 1);
stairs(time, yawrate_setpoint,'.-'); hold on; grid minor;
stairs(time_model_40ms, yawrate_setpoint_model_data, '.-'); hold off;
title('Yaw Controller Output');
@@ -151,7 +161,7 @@ ylabel('d\psi/dt (rad/s)');
legend('Log', 'Model', 'location', 'northwest');
% Plot yaw controller command
-subplot(2, 2, 2);
+ax2 = subplot(2, 2, 2);
stairs(time, yaw_command, '.-'); hold on; grid minor;
stairs(time_model_5ms, yaw_command_model_data, '.-'); hold off;
title('Yaw Command');
@@ -160,7 +170,7 @@ ylabel('Command');
legend('Log', 'Model', 'location', 'northwest');
% Plot yaw position
-subplot(2, 2, 3);
+ax3 = subplot(2, 2, 3);
stairs(time, yaw_value, '.-'); hold on; grid minor;
stairs(time_model_40ms, yaw_value_model_data, '.-'); hold off;
title('Yaw Position');
@@ -168,10 +178,12 @@ xlabel('Time (s)');
ylabel('Value (rad)');
legend('Log', 'Model', 'location', 'northwest');
+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');
@@ -179,7 +191,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');
@@ -187,7 +199,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');
@@ -195,7 +207,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');
@@ -205,29 +217,34 @@ linkaxes([ax1, ax2, ax3, ax4], 'xy');
%% Plot output of complimentary filter
-figure(8); 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_IMU * (180/pi), '.-');
-stairs(time_model_5ms, pitch_accel * (180/pi), '.-'); hold off;
+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;
title('Pitch Complementary Filter Output');
xlabel('Time (s)');
ylabel('Pitch Angle (degrees)');
legend('VRPN','IMU', 'Model', 'location', 'northwest');
-subplot(2, 1, 2);
+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), '.-'); hold off;
+%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');
+linkaxes([ax1, ax2], 'x');
+
%% Plot VRPN Position
figure(9); ax1 = subplot(3, 1, 1);
stairs(time, x_position, '.-'); 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)');
ylabel('X position');
@@ -235,7 +252,9 @@ legend('X position', 'X position model', 'X setpoint');
ax2 = subplot(3, 1, 2);
stairs(time, y_position, '.-'); hold on; grid minor;
-stairs(time_model_40ms, y_position_model_data, '.-');
+%stairs(time_model_40ms, y_position_model_data, '.-');
+stairs(time, x_setpoint, '.-');
+
title('Y position');
xlabel('Time (s)');
ylabel('Y position');
@@ -250,3 +269,45 @@ ylabel('Z position');
legend('Z position', 'Z position model', 'Y setpoint');
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;
+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;
+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;
+title('gyro z');
+xlabel('Time (s)');
+ylabel('r (rad/s)');
+
+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;
+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;
+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;
+title('accel z');
+xlabel('Time (s)');
+ylabel('accel (z)');
+
+linkaxes([ax1, ax2, ax3], 'x');
\ No newline at end of file
diff --git a/controls/model/modelParameters.m b/controls/model/modelParameters.m
index 20dad9510c22a96dec30005120ada0f9c7aebd34..3ad190cf93f61fe0b5149b9e8f70b9ab297e75a4 100644
--- a/controls/model/modelParameters.m
+++ b/controls/model/modelParameters.m
@@ -18,6 +18,7 @@
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
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
Kq = 96.3422; % Motor torque constant
Kv = 96.3422; % Motor back emf constant
@@ -31,6 +32,8 @@
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
+ Ki_mahony = 0.001; % Integral term for mahony filter
% Determine Initial Conditions
@@ -114,7 +117,7 @@ if logAnalysisToggle == 1
% Determine pitch error
pitch_setpoint = dataStruct.X_pos_PID_Correction.data;
- pitch_value = dataStruct.Pitch_Constant.data;
+ pitch_value = dataStruct.Pitch_trim_add_Sum.data;
pitch_error = timeseries(pitch_setpoint - pitch_value, time);
% Determine roll error
@@ -175,10 +178,10 @@ 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;
+ %z_command = dataStruct.RC_Throttle_Constant.data;
% Pull the measurements from the complimentary filter
- pitch_measured_IMU = dataStruct.Pitch_Constant.data;
+ pitch_measured_IMU = dataStruct.Pitch_trim_add_Sum.data;
roll_measured_IMU = dataStruct.Roll_Constant.data;
IMU_angle_arr = ...
[roll_measured_IMU, pitch_measured_IMU];
@@ -188,37 +191,37 @@ 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;
+% % 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;
end
\ No newline at end of file
diff --git a/controls/model/test_model.slx b/controls/model/test_model.slx
index 56971125dc1d50445e083b3e0f8b6d3187957d3b..62dc0a21547c1eff7b1e558e74d80030e2955325 100644
Binary files a/controls/model/test_model.slx and b/controls/model/test_model.slx differ