diff --git a/controls/DataAnalysisTool/Tool/DataAnalysis.m b/controls/DataAnalysisTool/Tool/DataAnalysis.m
index a7b87317346c0d2289901a5cefe5c6aec86e4ca8..b8cb1710ed7405ed8798878d1e2a0e64cd135f7b 100644
--- a/controls/DataAnalysisTool/Tool/DataAnalysis.m
+++ b/controls/DataAnalysisTool/Tool/DataAnalysis.m
@@ -51,9 +51,9 @@ backgnd = [1 1 1]; % rgb array for background color of the plot
%% DO NOT MODIFY %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-fpath = '';
+fpath = '/local/ucart/Documents/MicroCART_17-18/groundStation/logs/';
if(isempty(fname))
- [fname, fpath] = uigetfile('.txt','Select log file');
+ [fname, fpath] = uigetfile('.txt','Select log file', fpath);
end
% storing file options in the structure
@@ -100,4 +100,6 @@ plot_data(expData, params.plotting);
% main.params = params;
% main.expData = expData;
% clearvars -except main;
-% save main main;
\ No newline at end of file
+% save main main;
+
+clear plot;
\ No newline at end of file
diff --git a/controls/DataAnalysisTool/Tool/simplePlots.m b/controls/DataAnalysisTool/Tool/simplePlots.m
index a1087dfda7ccb05ff6f95b7edc4d465056ba6f77..c969045e174281faa600fa02bb38089cb586375e 100644
--- a/controls/DataAnalysisTool/Tool/simplePlots.m
+++ b/controls/DataAnalysisTool/Tool/simplePlots.m
@@ -40,22 +40,30 @@ linkaxes([ax1, ax2], 'x');
ax2 = subplot(2,2,1);
plot(expData.Time.data, expData.X_Setpoint_Constant.data - expData.VRPN_X_Constant.data);
title('X error');
+ylabel('meters');
+xlabel('time (s)');
ax1 = subplot(2,2,2);
plot(expData.Time.data, expData.X_pos_PID_Correction.data);
-title('x output');
+title('x controller output');
+ylabel('rad');
+xlabel('time (s)');
ax3 = subplot(2,2,3);
plot(expData.Time.data, expData.Pitch_PID_Correction.data); hold on;
-plot(expData.Time.data, expData.VRPN_Pitch_Constant.data .* 10);
+%plot(expData.Time.data, expData.VRPN_Pitch_Constant.data .* 10);
title('pitch output');
-legend('output', 'Pitch x10');
+ylabel('rad/s');
+%legend('output', 'Pitch x10');
+xlabel('time (s)');
ax4 = subplot(2,2,4);
plot(expData.Time.data, expData.Pitch_Rate_PID_Correction.data); hold on;
-plot(expData.Time.data, expData.gyro_y.data .* 100000);
-legend('output', 'Pitch rate x100000');
+%plot(expData.Time.data, expData.gyro_y.data * 0.1);
+%legend('output', 'Pitch rate x100000');
+ylabel('Normalized PWM');
title('pitch rate output');
+xlabel('time (s)');
linkaxes([ax1, ax2, ax3, ax4], 'x');
%% X pos controller flow
@@ -97,7 +105,7 @@ all_motors = expData.Signal_Mixer_MOTOR_0.data + expData.Signal_Mixer_MOTOR_1.da
expData.Signal_Mixer_MOTOR_2.data + expData.Signal_Mixer_MOTOR_3.data;
ax1 = subplot(1, 2, 1);
plot(expData.Time.data, all_motors ./ 4); hold on;
-plot(expData.Time.data, expData.RC_Throttle_Constant.data); hold on;
+%plot(expData.Time.data, expData.RC_Throttle_Constant.data); hold on;
plot(expData.Time.data, expData.Pitch_Rate_PID_Correction.data); hold on;
plot(expData.Time.data, expData.Roll_Rate_PID_Correction.data); hold on;
plot(expData.Time.data, expData.Yaw_Rate_PID_Correction.data);
@@ -109,11 +117,11 @@ linkaxes([ax1, ax2], 'x');
%%
ax1 = subplot(1, 2, 1);
-plot(expData.Time.data, expData.Pitch_Constant.data .* (180 / pi)); hold on; grid minor
+%plot(expData.Time.data, expData.Pitch_Constant.data .* (180 / pi)); hold on; grid minor
plot(expData.Time.data, expData.VRPN_Pitch_Constant.data .* (180 / pi));
legend('imu', 'vrpn');
ax2 = subplot(1, 2, 2);
-plot(expData.Time.data, expData.Roll_Constant.data .* (180 / pi)); hold on; grid minor
+%plot(expData.Time.data, expData.Roll_Constant.data .* (180 / pi)); hold on; grid minor
plot(expData.Time.data, expData.VRPN_Roll_Constant.data .* (180 / pi));
legend('imu', 'vrpn');
linkaxes([ax1, ax2], 'x');
@@ -163,8 +171,8 @@ linkaxes([ax1, ax2, ax3, ax4], 'x');
%% vel flow
figure;
ax2 = subplot(2,2,1);
-plot(expData.Time.data, expData.OF_Offset_Angle_Rotated_X.data - expData.RC_Pitch_Constant.data); hold on;
plot(expData.Time.data, expData.OF_Offset_Angle_Rotated_X.data); hold on;
+%plot(expData.Time.data, expData.OF_Offset_Angle_Rotated_X.data); hold on;
%plot(expData.Time.data, expData.X_Vel_Correction.data); hold on;
%plot(expData.Time.data, [0; raw_derivative]);
title('X velocity error');
@@ -187,6 +195,7 @@ title('pitch rate output');
linkaxes([ax1, ax2, ax3, ax4], 'x');
%%
+figure;
ax1 = subplot(2, 1, 1);
plot(expData.Time.data, expData.Alt_Setpoint_Constant.data - expData.VRPN_Alt_Constant.data); hold on;
plot(expData.Time.data, expData.Alt_Setpoint_Constant.data); hold on;
@@ -222,12 +231,12 @@ figure;
plot(expData.Time.data, expData.Lidar_Constant.data); hold on;
plot(expData.Time.data, expData.VRPN_Alt_Constant.data);
-angle = sqrt(expData.Roll_Constant.data.^2 + expData.VRPN_Pitch_Constant.data.^2);
-corrected = expData.Lidar_Constant.data .* cos(angle);
+%angle = sqrt(expData.Roll_Constant.data.^2 + expData.VRPN_Pitch_Constant.data.^2);
+%corrected = expData.Lidar_Constant.data .* cos(angle);
%plot(expData.Time.data, corrected);
-legend('lidar', 'vrpn', 'angle corrected');
+legend('lidar', 'vrpn');
-linkaxes([ax1, ax2], 'x');
+%linkaxes([ax1, ax2], 'x');
%% Sonar
filtered_sonar = [];
@@ -246,33 +255,159 @@ plot(expData.Time.data, -expData.Flow_Distance_Constant.data + alt_offset); hold
plot(expData.Time.data, expData.VRPN_Alt_Constant.data);
plot(expData.Time.data, -filtered_sonar + alt_offset);
legend('sonar', 'vrpn', 'dumb filter');
-%%
+%% THE ONE
figure;
-offsetX = -expData.OF_Integrate_X_Integrated.data(1) - expData.VRPN_X_Constant.data(1);
-offsetY = -expData.OF_Integrate_Y_Integrated.data(1) - expData.VRPN_Y_Constant.data(1);
+% offsetX = -expData.OF_Integrate_X_Integrated.data(1) - expData.VRPN_X_Constant.data(1);
+% offsetY = -expData.OF_Integrate_Y_Integrated.data(1) - expData.VRPN_Y_Constant.data(1);
+offsetX = 0;
+offsetY = 0;
-ax1 = subplot(2, 1, 1);
-plot(expData.Time.data, -expData.OF_Integrate_X_Integrated.data - offsetX); hold on;
+ax1 = subplot(3, 1, 1);
+plot(expData.Time.data, expData.OF_Integrate_X_Integrated.data - offsetX); hold on; grid minor
plot(expData.Time.data, expData.VRPN_X_Constant.data);
-legend('OF X Position', 'VRPN X Position');
+plot(expData.Time.data, expData.X_Setpoint_Constant.data);
+legend('OF X Position', 'VRPN X Position', 'X setpoint');
xlabel('Time (s)');
ylabel('Displacement (m)');
hold off;
-ax2 = subplot(2, 1, 2);
-plot(expData.Time.data, -expData.OF_Integrate_Y_Integrated.data - offsetY); hold on;
+ax2 = subplot(3, 1, 2);
+plot(expData.Time.data, expData.OF_Integrate_Y_Integrated.data - offsetY); hold on; grid minor
plot(expData.Time.data, expData.VRPN_Y_Constant.data);
-legend('OF Y Position', 'VRPN Y Position');
+plot(expData.Time.data, expData.Y_Setpoint_Constant.data);
+legend('OF Y Position', 'VRPN Y Position', 'Y setpoint');
xlabel('Time (s)');
ylabel('Displacement (m)');
hold off;
-linkaxes([ax1, ax2]);
+ax3 = subplot(3, 1, 3);
+plot(expData.Time.data, expData.Lidar_Constant.data); hold on; grid minor
+plot(expData.Time.data, expData.VRPN_Alt_Constant.data);
+plot(expData.Time.data, expData.Alt_Setpoint_Constant.data);
+legend('Lidar Z Position', 'VRPN Z Position', 'Z setpoint');
+xlabel('Time (s)');
+ylabel('Displacement (m)');
+hold off;
+
+linkaxes([ax1, ax2, ax3], 'x');
+
+
+linkaxes([ax1, ax2, ax3], 'x');
+%% Error Graphs
+figure;
+ax1 = subplot(2,1,1);
+plot(expData.Time.data, expData.X_Setpoint_Constant.data - expData.OF_Integrate_X_Integrated.data);
+title('X error');
+ax2 = subplot(2,1,2);
+plot(expData.Time.data, expData.Y_Setpoint_Constant.data - expData.OF_Integrate_Y_Integrated.data);
+title('Y error');
-%% Integarted gyro yaw
+%% Integarted gyro0.55 yaw
figure;
-plot(expData.Time.data, cumtrapz(expData.gyro_z.data + 0.0088)); hold on;
-plot(expData.Time.data, expData.Yaw_Constant.data); hold on;
+gyro_yaw = 0.005 * cumtrapz(expData.gyro_z.data + 0.0073);
+plot(expData.Time.data, (180/pi) * gyro_yaw); hold on;
+plot(expData.Time.data, expData.Yaw_Constant.data * 180/pi); hold on;
legend('Integrated gyro z', 'actual yaw');
+ylabel('Yaw (degrees)');
+xlabel('Time (s)');
+
+%%
+figure;
+angleOffset = 0.62204 + gyro_yaw;
+
+FlowVelX = expData.Flow_Vel_X_Constant.data.*cos(angleOffset) - expData.Flow_Vel_Y_Constant.data.*sin(angleOffset);
+FlowVelY = expData.Flow_Vel_X_Constant.data.*sin(angleOffset) + expData.Flow_Vel_Y_Constant.data.*cos(angleOffset);
+
+fc = 10;
+FlowVelX = BiquadFilter(FlowVelX, 200, fc);
+FlowVelY = BiquadFilter(FlowVelY, 200, fc);
+
+flowX = zeros(1, length(expData.Time.data));
+
+driftX = 0;
+driftY = 0;
+
+flowX(1) = expData.VRPN_X_Constant.data(1);
+for n = 2:length(flowX)
+ flowX(n) = flowX(n-1) + 0.005*(FlowVelX(n) - driftX);
+end
+
+flowY = zeros(1, length(expData.Time.data));
+flowY(1) = expData.VRPN_Y_Constant.data(1);
+for n = 2:length(flowY)
+ flowY(n) = flowY(n-1) + 0.005*(FlowVelY(n) - driftY);
+end
+
+ax1 = subplot(2, 1, 1);
+plot(expData.Time.data, flowX); hold on;
+plot(expData.Time.data, expData.VRPN_X_Constant.data);
+%legend('OF Integrated X Position');
+legend('OF Integrated X', 'VRPN X');
+%legend('OF Integrated X Position', 'Approximate Max X Position (measured with tape measure)');
+xlabel('Time (s)');
+ylabel('Position (m)');
+hold off;
+
+ax2 = subplot(2, 1, 2);
+plot(expData.Time.data, flowY); hold on;
+plot(expData.Time.data, expData.VRPN_Y_Constant.data);
+%legend('OF Integrated Y Position');
+legend('OF Integrated Y', 'VRPN Y');
+%legend('OF Integrated Y Position', 'Approximate Max Y Position (measured with tape measure)');
+xlabel('Time (s)');
+ylabel('Position (m)');
+hold off;
+
+linkaxes([ax1 ax2]);
+%%
+figure;
+ax1 = subplot(2, 1, 1);
+plot(expData.Time.data, expData.Flow_Quality_Constant.data);
+ax2 = subplot(2, 1, 2);
+plot(expData.Time.data, expData.Lidar_Constant.data); hold on;
+plot(expData.Time.data, expData.VRPN_Alt_Constant.data);
+linkaxes([ax1 ax2], 'x');
+
+%%
+figure;
+ax1 = subplot(3, 1, 1);
+plot(expData.Time.data, expData.mag_x.data);
+ax2 = subplot(3, 1, 2);
+plot(expData.Time.data, expData.mag_y.data);
+ax3 = subplot(3, 1, 3);
+plot(expData.Time.data, expData.mag_z.data);
+linkaxes([ax1 ax2 ax3], 'x');
+
+%%
+figure;
+filtX = BiquadFilter(expData.mag_x.data+33.9844, 200, 1);
+filtY = BiquadFilter(expData.mag_y.data-40.4922, 200, 1);
+
+magYaw = atan2(-filtY, -filtX);
+gyroYaw = cumtrapz(expData.gyro_z.data - 0.008) * 0.005;
+
+ax1 = subplot(3, 1, 1);
+plot(expData.Time.data, magYaw*180/pi);
+ax2 = subplot(3, 1, 2);
+plot(expData.Time.data, gyroYaw*180/pi);
+ax3 = subplot(3, 1, 3);
+plot(expData.Time.data, expData.Mag_Yaw_Constant.data*180/pi);
+
+linkaxes([ax1 ax2 ax3], 'x');
+
+%%
+mag = expData.mag_z.data;
+
+count = 0;
+for n = 2:length(mag)
+ if mag(n) == mag(n-1)
+ count = count + 1;
+ if count >= 10
+ disp(['Stall detected at index ' num2str(n)]);
+ end
+ else
+ count = 0;
+ end
+end
\ No newline at end of file
diff --git a/groundStation/gui/MicroCART/MicroCART.pro.user b/groundStation/gui/MicroCART/MicroCART.pro.user
deleted file mode 100644
index b87c55319792272b73342ccf2bffbc7f0a466fba..0000000000000000000000000000000000000000
--- a/groundStation/gui/MicroCART/MicroCART.pro.user
+++ /dev/null
@@ -1,216 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<!DOCTYPE QtCreatorProject>
-<!-- Written by QtCreator 4.0.1, 2017-04-21T10:58:17. -->
-<qtcreator>
- <data>
- <variable>EnvironmentId</variable>
- <value type="QByteArray">{ec588c71-c0cc-43f4-8233-a07fa24de8ad}</value>
- </data>
- <data>
- <variable>ProjectExplorer.Project.ActiveTarget</variable>
- <value type="int">0</value>
- </data>
- <data>
- <variable>ProjectExplorer.Project.EditorSettings</variable>
- <valuemap type="QVariantMap">
- <value type="bool" key="EditorConfiguration.AutoIndent">true</value>
- <value type="bool" key="EditorConfiguration.AutoSpacesForTabs">false</value>
- <value type="bool" key="EditorConfiguration.CamelCaseNavigation">true</value>
- <valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.0">
- <value type="QString" key="language">Cpp</value>
- <valuemap type="QVariantMap" key="value">
- <value type="QByteArray" key="CurrentPreferences">CppGlobal</value>
- </valuemap>
- </valuemap>
- <valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.1">
- <value type="QString" key="language">QmlJS</value>
- <valuemap type="QVariantMap" key="value">
- <value type="QByteArray" key="CurrentPreferences">QmlJSGlobal</value>
- </valuemap>
- </valuemap>
- <value type="int" key="EditorConfiguration.CodeStyle.Count">2</value>
- <value type="QByteArray" key="EditorConfiguration.Codec">UTF-8</value>
- <value type="bool" key="EditorConfiguration.ConstrainTooltips">false</value>
- <value type="int" key="EditorConfiguration.IndentSize">4</value>
- <value type="bool" key="EditorConfiguration.KeyboardTooltips">false</value>
- <value type="int" key="EditorConfiguration.MarginColumn">80</value>
- <value type="bool" key="EditorConfiguration.MouseHiding">true</value>
- <value type="bool" key="EditorConfiguration.MouseNavigation">true</value>
- <value type="int" key="EditorConfiguration.PaddingMode">1</value>
- <value type="bool" key="EditorConfiguration.ScrollWheelZooming">true</value>
- <value type="bool" key="EditorConfiguration.ShowMargin">false</value>
- <value type="int" key="EditorConfiguration.SmartBackspaceBehavior">0</value>
- <value type="bool" key="EditorConfiguration.SmartSelectionChanging">true</value>
- <value type="bool" key="EditorConfiguration.SpacesForTabs">true</value>
- <value type="int" key="EditorConfiguration.TabKeyBehavior">0</value>
- <value type="int" key="EditorConfiguration.TabSize">8</value>
- <value type="bool" key="EditorConfiguration.UseGlobal">true</value>
- <value type="int" key="EditorConfiguration.Utf8BomBehavior">1</value>
- <value type="bool" key="EditorConfiguration.addFinalNewLine">true</value>
- <value type="bool" key="EditorConfiguration.cleanIndentation">true</value>
- <value type="bool" key="EditorConfiguration.cleanWhitespace">true</value>
- <value type="bool" key="EditorConfiguration.inEntireDocument">false</value>
- </valuemap>
- </data>
- <data>
- <variable>ProjectExplorer.Project.PluginSettings</variable>
- <valuemap type="QVariantMap"/>
- </data>
- <data>
- <variable>ProjectExplorer.Project.Target.0</variable>
- <valuemap type="QVariantMap">
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{c6f8ca21-0eb9-4188-b2e8-fae8725afa1b}</value>
- <value type="int" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</value>
- <value type="int" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
- <value type="int" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
- <valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
- <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory"></value>
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
- <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">qmake</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">QtProjectManager.QMakeBuildStep</value>
- <value type="bool" key="QtProjectManager.QMakeBuildStep.LinkQmlDebuggingLibrary">false</value>
- <value type="QString" key="QtProjectManager.QMakeBuildStep.QMakeArguments"></value>
- <value type="bool" key="QtProjectManager.QMakeBuildStep.QMakeForced">false</value>
- <value type="bool" key="QtProjectManager.QMakeBuildStep.SeparateDebugInfo">false</value>
- <value type="bool" key="QtProjectManager.QMakeBuildStep.UseQtQuickCompiler">false</value>
- </valuemap>
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.1">
- <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
- <valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments">
- <value type="QString">-w</value>
- <value type="QString">-r</value>
- </valuelist>
- <value type="bool" key="Qt4ProjectManager.MakeStep.Clean">false</value>
- <value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments"></value>
- <value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
- </valuemap>
- <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">2</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
- </valuemap>
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
- <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
- <valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments">
- <value type="QString">-w</value>
- <value type="QString">-r</value>
- </valuelist>
- <value type="bool" key="Qt4ProjectManager.MakeStep.Clean">true</value>
- <value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments">clean</value>
- <value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
- </valuemap>
- <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
- </valuemap>
- <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
- <value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
- <valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Release</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.Qt4BuildConfiguration</value>
- <value type="int" key="Qt4ProjectManager.Qt4BuildConfiguration.BuildConfiguration">0</value>
- <value type="bool" key="Qt4ProjectManager.Qt4BuildConfiguration.UseShadowBuild">true</value>
- </valuemap>
- <value type="int" key="ProjectExplorer.Target.BuildConfigurationCount">1</value>
- <valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
- <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
- </valuemap>
- <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy locally</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
- </valuemap>
- <value type="int" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
- <valuemap type="QVariantMap" key="ProjectExplorer.Target.PluginSettings"/>
- <valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
- <value type="bool" key="Analyzer.QmlProfiler.AggregateTraces">false</value>
- <value type="bool" key="Analyzer.QmlProfiler.FlushEnabled">false</value>
- <value type="uint" key="Analyzer.QmlProfiler.FlushInterval">1000</value>
- <value type="QString" key="Analyzer.QmlProfiler.LastTraceFile"></value>
- <value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
- <valuelist type="QVariantList" key="Analyzer.Valgrind.AddedSuppressionFiles"/>
- <value type="bool" key="Analyzer.Valgrind.Callgrind.CollectBusEvents">false</value>
- <value type="bool" key="Analyzer.Valgrind.Callgrind.CollectSystime">false</value>
- <value type="bool" key="Analyzer.Valgrind.Callgrind.EnableBranchSim">false</value>
- <value type="bool" key="Analyzer.Valgrind.Callgrind.EnableCacheSim">false</value>
- <value type="bool" key="Analyzer.Valgrind.Callgrind.EnableEventToolTips">true</value>
- <value type="double" key="Analyzer.Valgrind.Callgrind.MinimumCostRatio">0.01</value>
- <value type="double" key="Analyzer.Valgrind.Callgrind.VisualisationMinimumCostRatio">10</value>
- <value type="bool" key="Analyzer.Valgrind.FilterExternalIssues">true</value>
- <value type="int" key="Analyzer.Valgrind.LeakCheckOnFinish">1</value>
- <value type="int" key="Analyzer.Valgrind.NumCallers">25</value>
- <valuelist type="QVariantList" key="Analyzer.Valgrind.RemovedSuppressionFiles"/>
- <value type="int" key="Analyzer.Valgrind.SelfModifyingCodeDetection">1</value>
- <value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
- <value type="bool" key="Analyzer.Valgrind.ShowReachable">false</value>
- <value type="bool" key="Analyzer.Valgrind.TrackOrigins">true</value>
- <value type="QString" key="Analyzer.Valgrind.ValgrindExecutable">valgrind</value>
- <valuelist type="QVariantList" key="Analyzer.Valgrind.VisibleErrorKinds">
- <value type="int">0</value>
- <value type="int">1</value>
- <value type="int">2</value>
- <value type="int">3</value>
- <value type="int">4</value>
- <value type="int">5</value>
- <value type="int">6</value>
- <value type="int">7</value>
- <value type="int">8</value>
- <value type="int">9</value>
- <value type="int">10</value>
- <value type="int">11</value>
- <value type="int">12</value>
- <value type="int">13</value>
- <value type="int">14</value>
- </valuelist>
- <value type="int" key="PE.EnvironmentAspect.Base">2</value>
- <valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">MicroCART</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.Qt4RunConfiguration:/home/jake/Microcart_17-18/groundStation/gui/MicroCART/MicroCART.pro</value>
- <value type="bool" key="QmakeProjectManager.QmakeRunConfiguration.UseLibrarySearchPath">true</value>
- <value type="QString" key="Qt4ProjectManager.Qt4RunConfiguration.CommandLineArguments"></value>
- <value type="QString" key="Qt4ProjectManager.Qt4RunConfiguration.ProFile">MicroCART.pro</value>
- <value type="bool" key="Qt4ProjectManager.Qt4RunConfiguration.UseDyldImageSuffix">false</value>
- <value type="QString" key="Qt4ProjectManager.Qt4RunConfiguration.UserWorkingDirectory"></value>
- <value type="QString" key="Qt4ProjectManager.Qt4RunConfiguration.UserWorkingDirectory.default">/home/jake/Microcart_17-18/groundStation/gui/MicroCART</value>
- <value type="uint" key="RunConfiguration.QmlDebugServerPort">3768</value>
- <value type="bool" key="RunConfiguration.UseCppDebugger">false</value>
- <value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
- <value type="bool" key="RunConfiguration.UseMultiProcess">false</value>
- <value type="bool" key="RunConfiguration.UseQmlDebugger">false</value>
- <value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
- </valuemap>
- <value type="int" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
- </valuemap>
- </data>
- <data>
- <variable>ProjectExplorer.Project.TargetCount</variable>
- <value type="int">1</value>
- </data>
- <data>
- <variable>ProjectExplorer.Project.Updater.FileVersion</variable>
- <value type="int">18</value>
- </data>
- <data>
- <variable>Version</variable>
- <value type="int">18</value>
- </data>
-</qtcreator>
diff --git a/groundStation/scripts/parameterize.sh b/groundStation/scripts/parameterize.sh
index 591656810626bb7867698124adb50f46bc5e6579..9554cc5d7af53de49e1612e0766f3bb85b8d49b9 100755
--- a/groundStation/scripts/parameterize.sh
+++ b/groundStation/scripts/parameterize.sh
@@ -1,10 +1,9 @@
-cd ..
-./addnode 0 "Zero"
-./setparam "zero" 0 0
-./setsource "signal mixer" "pitch" "zero" 0
-./setsource "signal mixer" "roll" "zero" 0
-./setsource "signal mixer" "yaw" "zero" 0
-./addnode 0 "PWM_VAL"
-./setparam "pwm_val" 0 100000
-./setsource "signal mixer" "throttle" "pwm_val" 0
+./setparam 'x pos pid' 0 -0.015
+./setparam 'x pos pid' 1 -0.0075
+./setparam 'x pos pid' 2 -0.08
+./setparam 'x pos pid' 3 0.94
+./setparam 'y pos pid' 0 0.015
+./setparam 'y pos pid' 1 0.0075
+./setparam 'y pos pid' 2 0.08
+./setparam 'y pos pid' 3 0.94
diff --git a/groundStation/scripts/touch_down.sh b/groundStation/scripts/touch_down.sh
index 79a4bc11dc49dce027e4438eae05aa819b512c78..7098126f21074c3c534097fcc391eee352ea3a7a 100755
--- a/groundStation/scripts/touch_down.sh
+++ b/groundStation/scripts/touch_down.sh
@@ -1,6 +1,6 @@
#! /bin/bash
-cut_off="-0.15"
+cut_off="-0.17"
regex='[+-]?[0-9]+\.?[0-9]*$'
diff --git a/groundStation/src/backend/backend.c b/groundStation/src/backend/backend.c
index f384a6a1a96f59985fabe29fa4a431678727b31a..cad01ff742bd0541e33089e7483411bce4cdb22e 100644
--- a/groundStation/src/backend/backend.c
+++ b/groundStation/src/backend/backend.c
@@ -103,7 +103,8 @@ struct timeval timeArr[MAX_HASH_SIZE];
// global variables
static volatile int keepRunning = 1;
-const char *TRACKER_IP = "UAV@192.168.0.120:3883";
+//const char *TRACKER_IP = "UAV@192.168.0.120:3883"; // If using 3050 lab computer to run vrpn
+const char *TRACKER_IP = "UAV@192.168.0.194:3883"; // If using Eric's old laptop to run vrpn
static int zyboSocket;
static int backendSocket;
struct ucart_vrpn_tracker * tracker = NULL;
diff --git a/quad/src/quad_app/control_algorithm.c b/quad/src/quad_app/control_algorithm.c
index 7654903b85eca5702a0a199c5e923a68b05dd8ec..1d6fad4bf0a9d961a1f1b6f2215495a3d06dfa37 100644
--- a/quad/src/quad_app/control_algorithm.c
+++ b/quad/src/quad_app/control_algorithm.c
@@ -15,7 +15,7 @@
#define USE_LIDAR
#define USE_OF
-#define USE_FAKE_YAW
+#define USE_MAG_YAW
#define MAX_VALID_LIDAR (10.0) // Maximum valid distance to read from LiDAR to update
//10 degrees
@@ -127,6 +127,9 @@ int control_algorithm_init(parameter_t * ps)
// gyroscope z integrator
ps->gyro_yaw = graph_add_defined_block(graph, BLOCK_INTEGRATOR, "Integrated gyro z");
+ //Complementary yaw
+ ps->mag_yaw = graph_add_defined_block(graph, BLOCK_CONSTANT, "Mag Yaw");
+
ps->mixer = graph_add_defined_block(graph, BLOCK_MIXER, "Signal Mixer");
ps->angle_time = graph_add_defined_block(graph, BLOCK_CONSTANT, "Ts_IMU");
@@ -153,7 +156,7 @@ int control_algorithm_init(parameter_t * ps)
graph_set_source(graph, ps->roll_r_pid, PID_CUR_POINT, ps->gyro_x, CONST_VAL);
graph_set_source(graph, ps->roll_r_pid, PID_DT, ps->angle_time, CONST_VAL);
graph_set_source(graph, ps->roll_pid, PID_CUR_POINT, ps->cur_roll, CONST_VAL);
- //graph_set_source(graph, ps->roll_pid, PID_CUR_POINT, ps->vrpn_roll, CONST_VAL);
+ //graph_set_source(graph, ps->roll_pid, PID_CUR_POINT, ps->vrpn_roll, CONST_VAL);USE_FAKE_YAW
graph_set_source(graph, ps->roll_pid, PID_DT, ps->angle_time, CONST_VAL);
// Connect yaw PID chain
@@ -178,8 +181,8 @@ int control_algorithm_init(parameter_t * ps)
graph_set_source(graph, ps->x_vel_clamp, BOUNDS_IN, ps->x_pos_pid, PID_CORRECTION);
// X/Y global to local conversion
-#ifdef USE_FAKE_YAW
- graph_set_source(graph, ps->yaw_correction, ROT_YAW, ps->gyro_yaw, CONST_VAL);
+#ifdef USE_MAG_YAW
+ graph_set_source(graph, ps->yaw_correction, ROT_YAW, ps->mag_yaw, CONST_VAL);
#else
graph_set_source(graph, ps->yaw_correction, ROT_YAW, ps->cur_yaw, CONST_VAL);
#endif
@@ -229,8 +232,8 @@ int control_algorithm_init(parameter_t * ps)
// X/Y global to local conversion
-#ifdef USE_FAKE_YAW
- graph_set_source(graph, ps->yaw_correction, ROT_YAW, ps->gyro_yaw, CONST_VAL);
+#ifdef USE_MAG_YAW
+ graph_set_source(graph, ps->yaw_correction, ROT_YAW, ps->mag_yaw, CONST_VAL);
#else
graph_set_source(graph, ps->yaw_correction, ROT_YAW, ps->cur_yaw, CONST_VAL);
#endif
@@ -250,9 +253,9 @@ int control_algorithm_init(parameter_t * ps)
graph_set_source(graph, ps->throttle_trim_add, ADD_SUMMAND1, ps->alt_pid, PID_CORRECTION);
graph_set_source(graph, ps->throttle_trim_add, ADD_SUMMAND2, ps->throttle_trim, CONST_VAL);
// Yaw angle
-#ifdef USE_FAKE_YAW
+#ifdef USE_MAG_YAW
graph_set_source(graph, ps->yaw_pid, PID_DT, ps->angle_time, CONST_VAL);
- graph_set_source(graph, ps->yaw_pid, PID_CUR_POINT, ps->gyro_yaw, CONST_VAL);
+ graph_set_source(graph, ps->yaw_pid, PID_CUR_POINT, ps->mag_yaw, CONST_VAL);
#else
graph_set_source(graph, ps->yaw_pid, PID_DT, ps->pos_time, CONST_VAL);
graph_set_source(graph, ps->yaw_pid, PID_CUR_POINT, ps->cur_yaw, CONST_VAL);
@@ -271,8 +274,8 @@ int control_algorithm_init(parameter_t * ps)
graph_set_source(graph, ps->mixer, MIXER_YAW, ps->clamp_d_pwmY, BOUNDS_OUT);
// Connect optical flow
-#ifdef USE_FAKE_YAW
- graph_set_source(graph, ps->of_angle_corr, ROT_YAW, ps->gyro_yaw, ADD_SUM);
+#ifdef USE_MAG_YAW
+ graph_set_source(graph, ps->of_angle_corr, ROT_YAW, ps->mag_yaw, ADD_SUM);
#else
graph_set_source(graph, ps->of_angle_corr, ROT_YAW, ps->cur_yaw, ADD_SUM);
#endif
@@ -463,6 +466,7 @@ int control_algorithm_init(parameter_t * ps)
// Sensor values
graph_set_param_val(graph, ps->cur_pitch, CONST_SET, sensor_struct->pitch_angle_filtered);
graph_set_param_val(graph, ps->cur_roll, CONST_SET, sensor_struct->roll_angle_filtered);
+ graph_set_param_val(graph, ps->mag_yaw, CONST_SET, sensor_struct->yaw_angle_filtered);
graph_set_param_val(graph, ps->gyro_y, CONST_SET, sensor_struct->gyr_y);
graph_set_param_val(graph, ps->gyro_x, CONST_SET, sensor_struct->gyr_x);
graph_set_param_val(graph, ps->gyro_z, CONST_SET, sensor_struct->gyr_z);
diff --git a/quad/src/quad_app/log_data.c b/quad/src/quad_app/log_data.c
index dadb8194d29a6c6c677f4aea810ae9cfed1e32a2..94566042e3ddc79352fe1d2ba89b49780ceacfa0 100644
--- a/quad/src/quad_app/log_data.c
+++ b/quad/src/quad_app/log_data.c
@@ -119,6 +119,7 @@ void initialize_logging(log_t* log_struct, parameter_t* ps) {
addOutputToLog(log_struct, ps->of_integ_x, INTEGRATED, m);
addOutputToLog(log_struct, ps->of_integ_y, INTEGRATED, m);
addOutputToLog(log_struct, ps->gyro_yaw, INTEGRATED, rad);
+ addOutputToLog(log_struct, ps->mag_yaw, CONST_VAL, rad);
addParamToLog(log_struct, ps->cur_roll, CONST_VAL, rad);
addParamToLog(log_struct, ps->cur_yaw, CONST_VAL, rad);
@@ -135,7 +136,7 @@ void initialize_logging(log_t* log_struct, parameter_t* ps) {
addParamToLog(log_struct, ps->flow_quality, CONST_VAL, "none");
// TODO: Make this not stupid. Adding 6 for IMU and 1 for timestamp
- row_size = n_outputs + n_params + 6 + 1;
+ row_size = n_outputs + n_params + 9 + 1;
size_t needed_memory = sizeof(float) * row_size * LOG_STARTING_SIZE;
logArray = malloc(needed_memory);
if (!logArray) { // malloc failed
@@ -161,6 +162,10 @@ int log_data(log_t* log_struct, parameter_t* ps)
thisRow[offset++] = log_struct->gam.gyro_xVel_p;
thisRow[offset++] = log_struct->gam.gyro_yVel_q;
thisRow[offset++] = log_struct->gam.gyro_zVel_r;
+ thisRow[offset++] = log_struct->gam.mag_x;
+ thisRow[offset++] = log_struct->gam.mag_y;
+ thisRow[offset++] = log_struct->gam.mag_z;
+
int i;
for (i = 0; i < n_params; i++) {
@@ -218,7 +223,7 @@ void printLogging(struct CommDriver *comm, log_t* log_struct, parameter_t* ps, r
}
// Header names for the pre-defined values
- safe_sprintf_cat(&buf, "%%Time\taccel_x\taccel_y\taccel_z\tgyro_x\tgyro_y\tgyro_z");
+ safe_sprintf_cat(&buf, "%%Time\taccel_x\taccel_y\taccel_z\tgyro_x\tgyro_y\tgyro_z\tmag_x\tmag_y\tmag_z");
// Print all the recorded block parameters
for (i = 0; i < n_params; i++) {
@@ -239,7 +244,7 @@ void printLogging(struct CommDriver *comm, log_t* log_struct, parameter_t* ps, r
// Send units header
buf.size = 0;
- safe_sprintf_cat(&buf, "&s\tG\tG\tG\trad/s\trad/s\trad/s"); // The pre-defined ones
+ safe_sprintf_cat(&buf, "&s\tG\tG\tG\trad/s\trad/s\trad/s\tuT\tuT\tuT"); // The pre-defined ones
safe_sprintf_cat(&buf, units_output.str);
safe_sprintf_cat(&buf, units_param.str);
safe_sprintf_cat(&buf, "\n");
diff --git a/quad/src/quad_app/sensor_processing.c b/quad/src/quad_app/sensor_processing.c
index 1c6afabe186b4a6c82ae2048fd955a46dc767497..219a6bea3ef5af6526bfe5b14f4d8db0c5e8d2d8 100644
--- a/quad/src/quad_app/sensor_processing.c
+++ b/quad/src/quad_app/sensor_processing.c
@@ -14,9 +14,15 @@
#include <math.h>
#define ALPHA 0.99
+#define YAW_ALPHA (0.995)
#define PX4FLOW_QUAL_MIN (100)
#define PX4FLOW_VEL_DECAY (0.99)
+#define MAG_OFFSET_X (-33.9844)
+#define MAG_OFFSET_Y (40.4922)
+
+#define GYRO_Z_OFFSET (0.0073)
+
int sensor_processing_init(sensor_t* sensor_struct) {
float a0 = 0.0200833310260;
float a1 = 0.0401666620520;
@@ -33,6 +39,15 @@ int sensor_processing_init(sensor_t* sensor_struct) {
float vel_b2 = 0.6853;
sensor_struct->flow_x_filt = filter_make_state(vel_a0, vel_a1, vel_a2, vel_b1, vel_b2);
sensor_struct->flow_y_filt = filter_make_state(vel_a0, vel_a1, vel_a2, vel_b1, vel_b2);
+
+ float mag_a0 = 2.3921e-4;
+ float mag_a1 = 4.7841e-4;
+ float mag_a2 = 2.3921e-4;
+ float mag_b1 = -1.9281;
+ float mag_b2 = 0.9391;
+ sensor_struct->mag_x_filt = filter_make_state(mag_a0, mag_a1, mag_a2, mag_b1, mag_b2);
+ sensor_struct->mag_y_filt = filter_make_state(mag_a0, mag_a1, mag_a2, mag_b1, mag_b2);
+
return 0;
}
@@ -213,6 +228,15 @@ int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_se
last_lidar = this_lidar;
sensor_struct->lidar_altitude = filtered_alt;
+ //Magnetometer filter
+ float magX_filt = biquad_execute(&sensor_struct->mag_x_filt, gam->mag_x - MAG_OFFSET_X);
+ float magY_filt = biquad_execute(&sensor_struct->mag_y_filt, gam->mag_y - MAG_OFFSET_Y);
+ float mag_yaw = atan2(-magY_filt, -magX_filt);
+
+ //Heading complementary filter
+ sensor_struct->yaw_angle_filtered = YAW_ALPHA * (sensor_struct->yaw_angle_filtered +
+ (sensor_struct->psi_dot)*get_last_loop_time()) + (1. - YAW_ALPHA) * mag_yaw;
+
return 0;
}
diff --git a/quad/src/quad_app/type_def.h b/quad/src/quad_app/type_def.h
index a16efb123839049dc70b5bfc50c24eccb67cd6ae..280da1a58e12c09f32b72e23f9b217588b3479f5 100644
--- a/quad/src/quad_app/type_def.h
+++ b/quad/src/quad_app/type_def.h
@@ -102,15 +102,18 @@ typedef struct gam {
float accel_roll;
float accel_pitch;
- // MAG
- //Xint16 raw_mag_x, raw_mag_y, raw_mag_z;
-
- float heading; // In degrees
+ //float heading; // In degrees
float mag_x; //Magnetic north: ~50 uT
float mag_y;
float mag_z;
+ float magX_correction;
+ float magY_correction;
+ float magZ_correction;
+
+ int magDRDYCount;
+
SensorError_t error;
} gam_t;
@@ -306,6 +309,7 @@ typedef struct sensor {
// Complementary filter outputs
float pitch_angle_filtered;
float roll_angle_filtered;
+ float yaw_angle_filtered;
// Z-axis value obtained from LiDAR
// Note that this is not distance, as our Z-axis points upwards.
@@ -322,6 +326,8 @@ typedef struct sensor {
struct biquadState accel_z_filt;
struct biquadState flow_x_filt;
struct biquadState flow_y_filt;
+ struct biquadState mag_x_filt;
+ struct biquadState mag_y_filt;
// Information obtained from optical flow sensor
px4flow_t optical_flow;
@@ -414,6 +420,7 @@ typedef struct parameter_t {
int of_trimmed_x; // Trimmed optical flow integrated value (of_integ_x + of_trim_x)
int of_trimmed_y;
int gyro_yaw; // Integrates the gyro z value
+ int mag_yaw; //Complementary filtered magnetometer/gyro yaw
} parameter_t;
/**
diff --git a/quad/xsdk_workspace/real_quad/.cproject b/quad/xsdk_workspace/real_quad/.cproject
index eff70df8bb1fb6819d306f20afe048666440a183..57f76856b0208ef5824dd1bc3f413b84b8f3c3cf 100644
--- a/quad/xsdk_workspace/real_quad/.cproject
+++ b/quad/xsdk_workspace/real_quad/.cproject
@@ -109,7 +109,7 @@
</tool>
<tool id="xilinx.gnu.arm.c.toolchain.compiler.release.85270120" name="ARM gcc compiler" superClass="xilinx.gnu.arm.c.toolchain.compiler.release">
<option defaultValue="gnu.c.optimization.level.more" id="xilinx.gnu.compiler.option.optimization.level.515686013" name="Optimization Level" superClass="xilinx.gnu.compiler.option.optimization.level" valueType="enumerated"/>
- <option id="xilinx.gnu.compiler.option.debugging.level.1121150517" name="Debug Level" superClass="xilinx.gnu.compiler.option.debugging.level" value="gnu.c.debugging.level.none" valueType="enumerated"/>
+ <option id="xilinx.gnu.compiler.option.debugging.level.1121150517" name="Debug Level" superClass="xilinx.gnu.compiler.option.debugging.level" value="gnu.c.debugging.level.max" valueType="enumerated"/>
<option id="xilinx.gnu.compiler.inferred.swplatform.includes.687694973" name="Software Platform Include Path" superClass="xilinx.gnu.compiler.inferred.swplatform.includes" valueType="includePath">
<listOptionValue builtIn="false" value="../../system_bsp/ps7_cortexa9_0/include"/>
</option>
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_imu.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_imu.c
index bf94f23894fcebfc6b76d97b49b9e4d869426226..c7f2bfff3b7751ebf40346d52431eca8e15352f3 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_imu.c
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_imu.c
@@ -13,6 +13,8 @@
#define MAG_READ_SIZE 6
#define MAG_BASE_ADDR 0x03
+#define MAG_DRDY_TIMEOUT (10)
+
#define RAD_TO_DEG 57.29578
#define DEG_TO_RAD 0.0174533
@@ -44,7 +46,7 @@
#define GYRO_X_BIAS 0.005f
#define GYRO_Y_BIAS -0.014f
-#define GYRO_Z_BIAS 0.0541f
+#define GYRO_Z_BIAS 0.0614//0.0541f
#define ACCEL_X_BIAS 0.023f
#define ACCEL_Y_BIAS 0.009f
@@ -53,6 +55,7 @@
int mpu9150_write(struct I2CDriver *i2c, u8 register_addr, u8 data);
int mpu9150_read(struct I2CDriver *i2c, u8* recv_buffer, u8 register_addr, int size);
+int mpu9150_calc_mag_sensitivity(struct IMUDriver *self, gam_t *gam);
int mpu9150_read_mag(struct IMUDriver *self, gam_t* gam);
int mpu9150_read_gyro_accel(gam_t* gam);
@@ -77,23 +80,57 @@ int zybo_imu_reset(struct IMUDriver *self, gam_t *gam) {
// Enable I2C bypass for AUX I2C (Magnetometer)
mpu9150_write(i2c, 0x37, 0x02);
- // Setup Mag
- mpu9150_write(i2c, 0x37, 0x02); //INT_PIN_CFG -- INT_LEVEL=0 ; INT_OPEN=0 ; LATCH_INT_EN=0 ; INT_RD_CLEAR=0 ; FSYNC_INT_LEVEL=0 ; FSYNC_INT_EN=0 ; I2C_BYPASS_EN=0 ; CLKOUT_EN=0
-
usleep(100000);
+ //Calculate magnetometer sensitivities
+ mpu9150_calc_mag_sensitivity(self, gam);
+
+ usleep(10000);
+
+ //Enable single measurement mode
+ mpu9150_write(i2c, 0x0A, 0x00);
+ mpu9150_write(i2c, 0x0A, 0x01);
+
int i;
- gam_t temp_gam;
// Do about 20 reads to warm up the device
for(i=0; i < 20; ++i){
- self->read(self, &temp_gam);
+ self->read(self, gam);
usleep(1000);
}
return 0;
}
+int mpu9150_calc_mag_sensitivity(struct IMUDriver *self, gam_t *gam) {
+ u8 buf[3];
+ u8 ASAX, ASAY, ASAZ;
+
+ // Quickly read from the factory ROM to get correction coefficents
+ int status = mpu9150_write(self->i2c, 0x0A, 0x0F);
+ if(status != 0) {
+ return status;
+ }
+
+ usleep(10000);
+
+ // Read raw adjustment values
+ status = mpu9150_read(self->i2c, buf, 0x10,3);
+ if(status != 0) {
+ return status;
+ }
+ ASAX = buf[0];
+ ASAY = buf[1];
+ ASAZ = buf[2];
+
+ // Set the correction coefficients
+ gam->magX_correction = (ASAX-128)*0.5/128 + 1;
+ gam->magY_correction = (ASAY-128)*0.5/128 + 1;
+ gam->magZ_correction = (ASAZ-128)*0.5/128 + 1;
+
+ return 0;
+}
+
int zybo_imu_read(struct IMUDriver *self, gam_t *gam) {
struct I2CDriver *i2c = self->i2c;
i16 raw_accel_x, raw_accel_y, raw_accel_z;
@@ -126,7 +163,7 @@ int zybo_imu_read(struct IMUDriver *self, gam_t *gam) {
gam->gyro_zVel_r = ((gyro_z / GYRO_SENS) * DEG_TO_RAD) + GYRO_Z_BIAS;
// Magnometer
- //mpu9150_read_mag(self, gam);
+ mpu9150_read_mag(self, gam);
return error;
}
@@ -174,56 +211,55 @@ int mpu9150_read(struct I2CDriver *i2c, u8* recv_buffer, u8 register_addr, int s
int mpu9150_read_mag(struct IMUDriver *self, gam_t* gam){
-
- static double magX_correction = -1, magY_correction, magZ_correction;
-
- struct I2CDriver *i2c = self->i2c;
-
- u8 mag_data[6];
- i16 raw_magX, raw_magY, raw_magZ;
-
- // Grab calibrations if not done already
- if(magX_correction == -1){
- u8 buf[3];
- u8 ASAX, ASAY, ASAZ;
-
- // Quickly read from the factory ROM to get correction coefficents
- mpu9150_write(i2c, 0x0A, 0x0F);
- usleep(10000);
-
- // Read raw adjustment values
- mpu9150_read(i2c, buf, 0x10,3);
- ASAX = buf[0];
- ASAY = buf[1];
- ASAZ = buf[2];
-
- // Set the correction coefficients
- magX_correction = (ASAX-128)*0.5/128 + 1;
- magY_correction = (ASAY-128)*0.5/128 + 1;
- magZ_correction = (ASAZ-128)*0.5/128 + 1;
- }
-
- // Set Mag to single read mode
- mpu9150_write(i2c, 0x0A, 0x01);
- usleep(10000);
- mag_data[0] = 0;
-
- // Keep checking if data is ready before reading new mag data
- while(mag_data[0] == 0x00){
- mpu9150_read(i2c, mag_data, 0x02, 1);
- }
-
- // Get mag data
- mpu9150_read(i2c, mag_data, 0x03, 6);
-
- raw_magX = (mag_data[1] << 8) | mag_data[0];
- raw_magY = (mag_data[3] << 8) | mag_data[2];
- raw_magZ = (mag_data[5] << 8) | mag_data[4];
-
- // Set magnetometer data to output
- gam->mag_x = raw_magX * magX_correction;
- gam->mag_y = raw_magY * magY_correction;
- gam->mag_z = raw_magZ * magZ_correction;
-
- return 0;
+ u8 mag_data[6];
+ u8 mag_status;
+ i16 raw_magX, raw_magY, raw_magZ;
+
+ int trigger = 0;
+
+ int status = mpu9150_read(self->i2c, mag_data, 0x02, 1);
+ if(status != 0) {
+ return status;
+ }
+
+ if(mag_data[0] & 0x01) {
+ // Get mag data
+ status = mpu9150_read(self->i2c, mag_data, 0x03, 6);
+ if(status != 0) {
+ return status;
+ }
+
+ status = mpu9150_read(self->i2c, &mag_status, 0x09, 1);
+ if(status != 0) {
+ return status;
+ }
+
+ raw_magX = (mag_data[1] << 8) | mag_data[0];
+ raw_magY = (mag_data[3] << 8) | mag_data[2];
+ raw_magZ = (mag_data[5] << 8) | mag_data[4];
+
+ // Set magnetometer data to output
+ gam->mag_x = raw_magX * gam->magX_correction;
+ gam->mag_y = raw_magY * gam->magY_correction;
+ gam->mag_z = raw_magZ * gam->magZ_correction;
+
+ trigger = 1;
+ }
+ else {
+ gam->magDRDYCount++;
+
+ if(gam->magDRDYCount > MAG_DRDY_TIMEOUT) {
+ gam->magDRDYCount = 0;
+
+ trigger = 1;
+ }
+ }
+
+ if(trigger) {
+ //Start next reading
+ mpu9150_write(self->i2c, 0x0A, 0x00);
+ mpu9150_write(self->i2c, 0x0A, 0x01);
+ }
+
+ return 0;
}
diff --git a/quad/xsdk_workspace/real_quad/src/main.c b/quad/xsdk_workspace/real_quad/src/main.c
index d715d8e6538906b99b38f21f50260d801540c4cf..198b48d057c468db94336fad51a42d6a5d7682ab 100644
--- a/quad/xsdk_workspace/real_quad/src/main.c
+++ b/quad/xsdk_workspace/real_quad/src/main.c
@@ -48,7 +48,7 @@ int main()
#ifdef RUN_TESTS
//test_zybo_mio7_led_and_system();
//test_zybo_i2c();
- //test_zybo_i2c_imu();
+ test_zybo_i2c_imu();
//test_zybo_i2c_px4flow();
//test_zybo_i2c_lidar();
//test_zybo_i2c_all();
diff --git a/website/content/files/Poster.pdf b/website/content/files/Poster.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..9986215a5a1bb63e9a33ad7c92b25c5e6d6beb65
Binary files /dev/null and b/website/content/files/Poster.pdf differ
diff --git a/website/content/pages/documents.md b/website/content/pages/documents.md
index d1e2971cfeb2d82a9ca7808e39e37d3c98025377..f0ef4ccfd9ec5a3031a86fca913cd276b7886db6 100644
--- a/website/content/pages/documents.md
+++ b/website/content/pages/documents.md
@@ -17,6 +17,10 @@ sortorder: 005
[Final Report](/files/492FinalReport.pdf)
+## Poster
+
+[Poster](/files/Poster.pdf)
+
## Weekly Reports
<iframe src="https://drive.google.com/embeddedfolderview?id=0BywzM7Q_7PUSeF8tdWpmMVN0eG8#list" width="100%" height="500" frameborder="0"></iframe>