-
Notifications
You must be signed in to change notification settings - Fork 0
/
imuviewer.m
92 lines (75 loc) · 2.63 KB
/
imuviewer.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
clear s; clear all;
%%addpath('./ThirdParty/STLRead');
% Dependencies: reads from the serial port the messages generated by the
% webserial_3d.ino firmware. Check the "Adafruit BNO055" library.
% change this accordingly (tested under Windows)
serialportname = "COM13";
baudrate = 9600;
imu_iscalibrated = 0;
f = figure(1);
[verts, faces, cindex] = teapotGeometry;
fv.vertices = verts;
fv.faces = faces;
%fv = stlread('../v5.stl');
fvtemp = fv;
%p = patch('Faces',faces,'Vertices',verts,'FaceColor','interp');
patch(fv,'FaceColor', [0.8 0.8 1.0], ...
'EdgeColor', 'none', ...
'FaceLighting', 'gouraud', ...
'AmbientStrength', 0.15);
% Add a camera light, and tone down the specular highlighting
camlight('headlight');
material('dull');
axis equal off
view(-151,30)
ButtonHandle = uicontrol('Style', 'PushButton', ...
'String', 'Stop loop', ...
'Callback', 'delete(gcbf)');
s = serialport(serialportname,baudrate);
q1 = 0.0;
q2 = 0.0;
q3 = 0.0;
q4 = 0.0;
qf = quaternion(q1,q2,q3,q4);
while(1)
dataline = readline(s);
dataparts = strsplit(dataline);
if (dataparts(1) == "Quaternion:") && (imu_iscalibrated == 1)
cla;
q1 = str2double(dataparts(2));
q2 = str2double(dataparts(3));
q3 = str2double(dataparts(4));
q4 = str2double(dataparts(5));
qf = quaternion(q1,q2,q3,q4);
%%verts2 = rotateframe(qf,verts);
fvtemp.vertices = rotatepoint(qf,fv.vertices);
%ax = plotTransforms([0 0 0],[0 0 0 0])
%%p = patch('Faces',faces,'Vertices',verts2,'FaceVertexCData',cindex,'FaceColor','interp');
% Add a camera light, and tone down the specular highlighting
patch(fvtemp,'FaceColor', [0.8 0.8 1.0], ...
'EdgeColor', 'none', ...
'FaceLighting', 'gouraud', ...
'AmbientStrength', 0.15);
camlight('headlight');
material('dull');
end
%%[q1,q2,q3,q4]
if dataparts(1) == "Calibration:"
%% Msg: Calibration: 3, 3, 0, 3
cal_sys = str2double(dataparts(2));
cal_gyro = str2double(dataparts(3));
cal_accel = str2double(dataparts(4));
cal_mag = str2double(dataparts(5));
if (cal_sys == 3) && (cal_gyro == 3) && ...
(cal_accel == 0) && (cal_mag == 3)
imu_iscalibrated = 1;
end
[cal_sys,cal_gyro,cal_accel,cal_mag]
end
if ~ishandle(ButtonHandle)
disp('Loop stopped by user');
clear s; % Cleaning the serial port handlers
clf;
break;
end
end