-
Notifications
You must be signed in to change notification settings - Fork 6
/
udpSocket2NavData.m
67 lines (59 loc) · 2.67 KB
/
udpSocket2NavData.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
function navData = udpSocket2NavData( udp_raw )
% UDPSOCKET2NAVDATA Convert UDP UTF-8 phone package to MATLAB class
% This functions take as input a UDP socket package and
% delivers a object of class navSensorsData which yields sensors data in a
% MATLAB friendly interface. Only works with the Android software called
% IMU+GPS-Streamsensor, freely available at Android Store.
% for more information, see <a href="matlab:
% web('http://lustosa-leandro.github.io')">the author's website</a>.
%% UDP package pre-processing
% converts raw data to a string
iStr = num2str(udp_raw,'%s');
% converts string to array of double data
iRawData = textscan(iStr,'%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f','Delimiter',',');
%% here we really handle data stream semantics
% instantiate object for capturing and abstracting sensor data
navSen = navSensorsData();
% first data is always timestamp (already in secs)
navSen.time = iRawData{1};
% from here on, depends on its frame identifier
byteCount = 2; % position in the frame
while ~isempty(iRawData{byteCount}) % are we in the end of the frame?
% if not, get identifier and execute associated action
switch iRawData{byteCount}
case 1 % GNSS Position
navSen.gnss_hasPosData = true;
navSen.gnss_lla(1) = iRawData{byteCount+1}*pi/180;
navSen.gnss_lla(2) = iRawData{byteCount+2}*pi/180;
navSen.gnss_lla(3) = iRawData{byteCount+3};
byteCount = byteCount + 4;
case 3 % accelerometer
navSen.acc_hasData = true;
navSen.acc(1) = iRawData{byteCount+1};
navSen.acc(2) = iRawData{byteCount+2};
navSen.acc(3) = iRawData{byteCount+3};
byteCount = byteCount + 4;
case 4 % rate-gyro
navSen.gyr_hasData = true;
navSen.gyr(1) = iRawData{byteCount+1};
navSen.gyr(2) = iRawData{byteCount+2};
navSen.gyr(3) = iRawData{byteCount+3};
byteCount = byteCount + 4;
case 5 % magnetometer
navSen.mag_hasData = true;
navSen.mag(1) = iRawData{byteCount+1};
navSen.mag(2) = iRawData{byteCount+2};
navSen.mag(3) = iRawData{byteCount+3};
byteCount = byteCount + 4;
case 6 % GNSS extra info (disregard for now)
byteCount = byteCount + 4;
case 7 % GNSS extra info (disregard for now)
byteCount = byteCount + 4;
case 8 % GNSS extra info (disregard for now)
byteCount = byteCount + 2;
otherwise
disp('Something stinky happened while reading sensors UDP frame')
end
end
navData = navSen;
end