-
Notifications
You must be signed in to change notification settings - Fork 6
/
connect.m
191 lines (162 loc) · 6.64 KB
/
connect.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
function connect(mobile_ip, local_port)
% CONNECT plots Android mobile sensors data by means of IMU+GPS-Stream app.
% usage: CONNECT(mobile_ip, local_port)
% for example: CONNECT('192.168.6.106',5555)
% for more information, see <a href="matlab:
% web('http://lustosa-leandro.github.io')">the author's website</a>.
%% binds udp socket to mobile phone
u = udp(mobile_ip,5554,'LocalPort',local_port);
fopen(u);
%% prepare interface
% close figure opened by last run, if any
figTag = 'AndroidPhoneNavFigure';
close(findobj('tag',figTag));
% create new figure for showing sensors graphs
hFig = figure('numbertitle', 'off', ...
'name', 'Android Phone Navigation Sensors', ...
'menubar','none', ...
'toolbar','none', ...
'resize', 'on', ...
'tag',figTag, ...
'renderer','painters', ...
'position',[200 200 1200 700]);
% create axes and titles for 9 sensors real-time plotting
hei_graph = 0.27;
hAxes.axis1 = createPanelAxisTitle(hFig,[0.01 0.05 0.30 hei_graph],'Accelerometer X'); % [X Y W H]
hAxes.axis2 = createPanelAxisTitle(hFig,[0.33 0.05 0.30 hei_graph],'Accelerometer Y'); % [X Y W H]
hAxes.axis3 = createPanelAxisTitle(hFig,[0.66 0.05 0.30 hei_graph],'Accelerometer Z'); % [X Y W H]
hAxes.axis4 = createPanelAxisTitle(hFig,[0.01 0.35 0.30 hei_graph],'Rate-Gyro X'); % [X Y W H]
hAxes.axis5 = createPanelAxisTitle(hFig,[0.33 0.35 0.30 hei_graph],'Rate-Gyro Y'); % [X Y W H]
hAxes.axis6 = createPanelAxisTitle(hFig,[0.66 0.35 0.30 hei_graph],'Rate-Gyro Z'); % [X Y W H]
hAxes.axis7 = createPanelAxisTitle(hFig,[0.01 0.65 0.30 hei_graph],'Magnetometer X'); % [X Y W H]
hAxes.axis8 = createPanelAxisTitle(hFig,[0.33 0.65 0.30 hei_graph],'Magnetometer Y'); % [X Y W H]
hAxes.axis9 = createPanelAxisTitle(hFig,[0.66 0.65 0.30 hei_graph],'Magnetometer Z'); % [X Y W H]
% creating animation handles
axes(hAxes.axis1); hLines.h1 = animatedline;
axes(hAxes.axis2); hLines.h2 = animatedline;
axes(hAxes.axis3); hLines.h3 = animatedline;
axes(hAxes.axis4); hLines.h4 = animatedline;
axes(hAxes.axis5); hLines.h5 = animatedline;
axes(hAxes.axis6); hLines.h6 = animatedline;
axes(hAxes.axis7); hLines.h7 = animatedline;
axes(hAxes.axis8); hLines.h8 = animatedline;
axes(hAxes.axis9); hLines.h9 = animatedline;
% exit button with text "exit"
uicontrol(hFig,'unit','pixel','style','pushbutton','string','Exit',...
'position',[100 10 50 25],'callback', ...
{@exitCallback,u});
% prepare interface for showing attitude result
hPlane = Aero.Animation;
hPlane.createBody('pa24-250_orange.ac','Ac3d');
hPlane.show();
posPlane = hPlane.Bodies{1}.Position;
%% initialize navigation solution
quat = [1 0 0 0]';
%% main loop
while true
%% check if UDP is dead, and terminate program if that is the case
if strcmp(u.status,'closed')
break; % something went wrong, let's stop everything!
end
%% reads a package of raw data
try
[udp_raw, charCount] = fread(u,8192);
catch
continue;
end
udp_raw = udp_raw';
% check if reading was succesful and deal with it if unsuccessful
if charCount <= 0
break; % something went wrong, let's stop everything!
end
% navData is an object for capturing and abstracting sensor data of class
% navSensorsData
try
navData = udpSocket2NavData(udp_raw);
catch
continue;
end
%% draws acc data in figure, if any
if navData.acc_hasData
addpoints(hLines.h1,navData.time,navData.acc(1));
addpoints(hLines.h2,navData.time,navData.acc(2));
addpoints(hLines.h3,navData.time,navData.acc(3));
windows_width = 5;
set(hAxes.axis1,'XLim',[navData.time-windows_width navData.time]);
set(hAxes.axis2,'XLim',[navData.time-windows_width navData.time]);
set(hAxes.axis3,'XLim',[navData.time-windows_width navData.time]);
drawnow;
end
%% draws rate-gyro data in figure, if any
if navData.gyr_hasData
addpoints(hLines.h4,navData.time,navData.gyr(1));
addpoints(hLines.h5,navData.time,navData.gyr(2));
addpoints(hLines.h6,navData.time,navData.gyr(3));
windows_width = 5;
set(hAxes.axis4,'XLim',[navData.time-windows_width navData.time]);
set(hAxes.axis5,'XLim',[navData.time-windows_width navData.time]);
set(hAxes.axis6,'XLim',[navData.time-windows_width navData.time]);
drawnow;
end
%% draws magneto data in figure, if any
if navData.mag_hasData
addpoints(hLines.h7,navData.time,navData.mag(1));
addpoints(hLines.h8,navData.time,navData.mag(2));
addpoints(hLines.h9,navData.time,navData.mag(3));
windows_width = 5;
set(hAxes.axis7,'XLim',[navData.time-windows_width navData.time]);
set(hAxes.axis8,'XLim',[navData.time-windows_width navData.time]);
set(hAxes.axis9,'XLim',[navData.time-windows_width navData.time]);
drawnow;
end
%% updates navigation solution
if navData.gyr_hasData || navData.acc_hasData || navData.mag_hasData
quat = attitude_estimator(quat, navData.time, navData.gyr, navData.acc, navData.mag);
[proll, ppitch, pyaw] = quat2angle(quat');
angles = 180/pi*[proll ppitch pyaw];
hPlane.moveBody(1,posPlane, angles);
end
end
%% closes UDP binding and figure
fclose(u);
close(hFig);
hPlane.delete();
end
%% create axis and title
% axis is created on uipanel container object. this allows more control
% over the layout of the GUI.
function hAxis = createPanelAxisTitle(hFig, pos, axisTitle)
% create panel
hPanel = uipanel('parent',hFig,'Position',pos,'Units','Normalized');
% create axis
hAxis = axes('position',[0 0 1 1],'Parent',hPanel);
hAxis.XTick = [];
hAxis.YTick = [];
hAxis.XColor = [1 1 1];
hAxis.YColor = [1 1 1];
% set video title using uicontrol. uicontrol is used so that text
% can be positioned in the context of the figure, not the axis.
titlePos = [pos(1)+0.02 pos(2)+pos(4) 0.3 0.03];
uicontrol('style','text',...
'String', axisTitle,...
'Units','Normalized',...
'Parent',hFig,'Position', titlePos,...
'BackgroundColor',hFig.Color);
end
%% exit button callback
% this callback function releases system objects and closes figure window.
function exitCallback(~,~,udp_descrip)
% close udp bindings (this forces window to close in main loop as well)
fclose(udp_descrip);
end
%% attitude estimator
% this function updates the navigation solution given new measurements
function new_quat = attitude_estimator(prev_quat, time, gyr, acc, mag)
% DEAR STUDENTS: IMPLEMENT THIS FUNCTION!!!
% time is given in secs
% gyr is given in rad/sec
% acc is given in m/s^2
% mag should be normalized for use
% the following line should be deleted and replaced for your code.
new_quat = prev_quat;
end