Added tests for complimentary filter and signal mixer for .txt log files

controls/model/loggingAnalysis/logAnalysis.m

@@ -37,6 +37,14 @@ 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
+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] );
+
+
+
 %% Plot x control structure
 
 % Plot lateral controller output
@@ -193,4 +201,24 @@ stairs(time_model_5ms, PWM3_model, '.-'); hold off;
 title('PWM3 Value');
 xlabel('Time (s)');
 ylabel('PWM3 Command');
-legend('Log', 'Model', 'location', 'northwest');
\ No newline at end of file
+legend('Log', 'Model', 'location', 'northwest');
+
+%% Plot output of complimentary filter
+
+figure(6); subplot(2, 1, 1);
+stairs(time, pitch_measured_IMU, '.-'); hold on; grid minor;
+stairs(time_model_5ms, pitch_accel, '.-'); hold off;
+title('Pitch Complimentary Filter Output');
+xlabel('Time (s)');
+ylabel('Pitch Angle (rad)');
+legend('Log', 'Model', 'location', 'northwest');
+
+subplot(2, 1, 2);
+stairs(time, roll_measured_IMU, '.-'); hold on; grid minor;
+stairs(time_model_5ms, roll_accel, '.-'); hold off;
+title('Roll Complimentary Filter Output');
+xlabel('Time (s)');
+ylabel('Roll Angle (rad)');
+legend('Log', 'Model', 'location', 'northwest');
+
+

controls/model/modelParameters.m

@@ -70,7 +70,8 @@ temp = 1;
 % Import Data and determine errors
 if logAnalysisToggle == 1 && temp == 0
     % Import Data to Workspace
-    data = importdata('loggingAnalysis/logFiles/logData.csv');
+    %data = importdata('loggingAnalysis/logFiles/logData.csv');
+    data = importdata('loggingAnalysis/logFiles/test.csv');
 
     % Set up time vector
     time = data.data(:, 1);
@@ -127,12 +128,32 @@ if logAnalysisToggle == 1 && temp == 0
     z_command = data.data(:, 8);
     yaw_command = data.data(:, 16);
     
+    %Create time series object for z command
+    throttle_command = timeseries(z_command, time);
+    
     % Determine signal mix PWM values
     PWM0 = data.data(:, 28);
     PWM1 = data.data(:, 29);
     PWM2 = data.data(:, 30);
     PWM3 = data.data(:, 31);
     
+    %Pull the measurements from the acceleratometer
+    raw_accel_data_x = data.data(:, 2);
+    raw_accel_data_y = data.data(:, 3);
+    raw_accel_data_z = data.data(:, 4);
+    raw_accel_data_arr = ...
+        [ raw_accel_data_x , raw_accel_data_y , raw_accel_data_z ];
+    raw_accel_data = timeseries( raw_accel_data_arr , time );
+    
+    %Pull the measurements from the gyroscope
+    raw_gyro_data_x = data.data(:, 5);
+    raw_gyro_data_y = data.data(:, 6);
+    raw_gyro_data_z = data.data(:, 7);
+    raw_gyro_data_arr = ...
+        [ raw_gyro_data_x , raw_gyro_data_y , raw_gyro_data_z ];
+    raw_gyro_data = timeseries( raw_gyro_data_arr , time );
+    
+    
     % Determine the initial height controller command
     % height_controlled_o = 1; % NEEDS UPDATED WHEN LOG FILES INCLUDE THROTTLE COMMAND
     
@@ -195,6 +216,7 @@ elseif logAnalysisToggle == 1 && temp == 1
     [dataStruct, headers] = parse_log_model(params.file.pathName);
     
     time = dataStruct.Time.data;
+    %time = dataStruct.time.data;
     runtime = max(time);
  
     % Determine x position error
@@ -253,7 +275,28 @@ elseif logAnalysisToggle == 1 && temp == 1
     PWM1 = dataStruct.Signal_Mixer_PWM_1.data;
     PWM2 = dataStruct.Signal_Mixer_PWM_2.data;
     PWM3 = dataStruct.Signal_Mixer_PWM_3.data;
-       
+    
+    %Pull the measurements from the acceleratometer
+    raw_accel_data_x = dataStruct.accel_x.data;
+    raw_accel_data_y = dataStruct.accel_y.data;
+    raw_accel_data_z = dataStruct.accel_z.data;
+    raw_accel_data_arr = ...
+        [ raw_accel_data_x , raw_accel_data_y , raw_accel_data_z ];
+    raw_accel_data = timeseries( raw_accel_data_arr , time );
+    
+    %Pull the measurements from the gyroscope
+    raw_gyro_data_x = dataStruct.gyro_x.data;
+    raw_gyro_data_y = dataStruct.gyro_y.data;
+    raw_gyro_data_z = dataStruct.gyro_z.data;
+    raw_gyro_data_arr = ...
+        [ raw_gyro_data_x , raw_gyro_data_y , raw_gyro_data_z ];
+    raw_gyro_data = timeseries( raw_gyro_data_arr , time );
+    
+    %Create time series object for z command
+    throttle_command = timeseries(z_command, time);
+    
+    %Pull the measurements from the complimentary filter
+    pitch_measured_IMU = dataStruct.Pitch_Constant.data;
+    roll_measured_IMU = dataStruct.Roll_Constant.data;
+    
 end
-
-