Bluetooth Low Energy 기기의 방향 추적하기
이 예제에서는 Bluetooth® Low Energy 통신을 사용한 기기 모션 센서 데이터로 기기 방향을 추적하는 방법을 보여줍니다.
하드웨어 설정
이 예제에서는 Nordic Thingy:52™ 기기를 사용합니다. Nordic Thingy:52는 9축 모션 센서가 있는 Bluetooth Low Energy 기기입니다. 이 기기는 원시 가속도계, 자이로스코프, 나침반 및 융합 데이터를 포함한 풍부한 센서 데이터를 제공합니다. 이 예제에서는 기기에서 계산된 회전 행렬을 사용하여 기기 방향을 추적합니다.
기기를 찾고 이에 연결하기
먼저, MATLAB®에서 Bluetooth Low Energy 기기를 찾아 그 기기가 연결을 지원하는지 확인합니다. blelist 함수가 애드버타이징하는 인근 Bluetooth Low Energy 주변 기기를 스캔합니다.
blelist
ans=20×5 table
1 "Thingy" "F2DF635320F6" -54 1×1 struct
2 "" "5AE98748DC34" -73 1×1 struct
3 "" "7A9762B423E0" -76 1×1 struct
4 "" "5E0EAEF93E78" -76 1×1 struct
5 "" "08534F9CC17B" -77 1×1 struct
6 "" "4323693660AC" -79 1×1 struct
7 "" "5386B1B9FCEC" -82 1×1 struct
8 "" "2D132D3ACD33" -83 1×1 struct
9 "" "537E555A0188" -84 1×1 struct
10 "" "237E6384E9BF" -87 1×1 struct
11 "" "2C0CA5F4793C" -88 1×1 struct
12 "" "55D810EF7331" -89 1×1 struct
13 "" "3A01FA8D3D18" -89 1×1 struct
14 "" "2084C6A7DA4D" -90 1×1 struct
⋮
기기가 MATLAB에서 발견되면 ble를 호출하여 기기에 연결합니다. 고유한 이름을 갖고 있는 경우 기기 이름을 지정하거나, 또는 기기 주소를 지정합니다.
b = ble("Thingy")b =
ble with properties:
Name: "Thingy"
Address: "F2DF635320F6"
Connected: 1
Services: [9×2 table]
Characteristics: [38×5 table]
Show services and characteristics
ble 객체의 Characteristics 속성에 액세스합니다. 기기에 "Motion Service" 서비스가 있으며, 이 서비스에는 "Rotation Matrix" 특성이 포함되어 있습니다.
b.Characteristics
ans=38×5 table
"Generic Access" "1800" "Device Name" "2A00" "Read"
"Generic Access" "1800" "Appearance" "2A01" "Read"
"Generic Access" "1800" "Peripheral Preferred Connection Parameters" "2A04" "Read"
"Generic Access" "1800" "Central Address Resolution" "2AA6" "Read"
"Generic Attribute" "1801" "Service Changed" "2A05" "Indicate"
"Thingy Configuration Service" "EF680100-9B35-4933-9B10-52FFA9740042" "Device Name" "EF680101-9B35-4933-9B10-52FFA9740042" 1×2 string
"Thingy Configuration Service" "EF680100-9B35-4933-9B10-52FFA9740042" "Advertising Parameters" "EF680102-9B35-4933-9B10-52FFA9740042" 1×2 string
"Thingy Configuration Service" "EF680100-9B35-4933-9B10-52FFA9740042" "Connection Parameters" "EF680104-9B35-4933-9B10-52FFA9740042" 1×2 string
"Thingy Configuration Service" "EF680100-9B35-4933-9B10-52FFA9740042" "Eddystone URL" "EF680105-9B35-4933-9B10-52FFA9740042" 1×2 string
"Thingy Configuration Service" "EF680100-9B35-4933-9B10-52FFA9740042" "Cloud Token" "EF680106-9B35-4933-9B10-52FFA9740042" 1×2 string
"Thingy Configuration Service" "EF680100-9B35-4933-9B10-52FFA9740042" "Firmware Version" "EF680107-9B35-4933-9B10-52FFA9740042" "Read"
"Thingy Configuration Service" "EF680100-9B35-4933-9B10-52FFA9740042" "MTU Request" "EF680108-9B35-4933-9B10-52FFA9740042" 1×2 string
"Thingy Configuration Service" "EF680100-9B35-4933-9B10-52FFA9740042" "NFC Tag Content" "EF680109-9B35-4933-9B10-52FFA9740042" 1×2 string
"Weather Station Service" "EF680200-9B35-4933-9B10-52FFA9740042" "Temperature" "EF680201-9B35-4933-9B10-52FFA9740042" "Notify"
⋮
센서 데이터 읽어오기
다음으로, 서비스와 특성 정보를 지정하여 "Rotation Matrix" 특성에 대한 객체를 만듭니다.
c = characteristic(b, "motion service", "rotation matrix")
c =
Characteristic with properties:
Name: "Rotation Matrix"
UUID: "EF680408-9B35-4933-9B10-52FFA9740042"
Attributes: "Notify"
Descriptors: [1x3 table]
DataAvailableFcn: []
Show descriptors
그런 다음 기기에서 현재 회전 행렬을 읽어옵니다.
data = read(c)
data = 1×18
253 63 255 255 15 1 255 255 0 64 8 0 240 254 247 255 253 63
Nordic Thingy:52 문서에 따르면 이 원시 데이터에는 3×3 행렬이 포함되어 있으며, 행렬의 각 요소는 2바이트로 전송된 16비트 정수입니다. 각 요소는 2개의 부호 및 지수 비트와 14개의 소수 비트로 구성된 부호 있는 부동소수점 숫자를 나타냅니다. 원시 데이터를 회전 행렬로 해석합니다.
% Prepare 4-by-4 transform matrix to plot later (assume the device has no % translation and only rotation) transformMatrix = eye(4); % Populate the transform matrix with 9 rotation matrix elements for row = 1:3 for column = 1:3 % Extract the 2 bytes representing the current element in the rotation matrix beginIndex = (row-1)*3 + (column-1); element = data(2*beginIndex + (1:2)); transformMatrix(row, column) = double(typecast(uint8(element), 'int16')) / (2^14); end end % Display the transform matrix disp(transformMatrix);
0.9998 -0.0001 0.0165 0
-0.0001 1.0000 0.0005 0
-0.0166 -0.0005 0.9998 0
0 0 0 1.0000
기기 방향 추적하기
기기 방향의 실시간 추적을 표시하기 위해 먼저 Nordic Thingy:52 기기를 나타내는 3차원 객체를 플로팅합니다.
% Create a 3-D plot ax = axes('XLim', [-1.5 1.5], 'YLim', [-1.5 1.5], 'ZLim', [-1 2]); xlabel(ax, 'X-axis'); ylabel(ax, 'Y-axis'); zlabel(ax, 'Z-axis'); % Reverse the 2 axis directions to match the device coordinate system set(ax, 'Zdir', 'reverse'); set(ax, 'Xdir', 'reverse'); grid on; view(3); % Define the surface color color = [0.3010 0.7450 0.9330]; % Create patches for all cube surfaces by specifying the four corners of each surface top = [-1 -1 1; 1 -1 1; 1 1 1; -1 1 1]; p(1) = patch(top(:,1), top(:,2), top(:,3), color); bottom = [-1 -1 0; 1 -1 0; 1 1 0; -1 1 0]; p(2) = patch(bottom(:,1), bottom(:,2), bottom(:,3), color); front = [1 -1 0; 1 1 0; 1 1 1; 1 -1 1]; p(3) = patch(front(:,1), front(:,2), front(:,3), color); back = [-1 -1 0; -1 1 0; -1 1 1; -1 -1 1]; p(4) = patch(back(:,1), back(:,2), back(:,3), color); left = [1 -1 0; -1 -1 0; -1 -1 1; 1 -1 1]; p(5) = patch(left(:,1), left(:,2), left(:,3), color); right = [1 1 0; -1 1 0; -1 1 1; 1 1 1]; p(6) = patch(right(:,1), right(:,2), right(:,3), color); mark = [0.9 -0.7 -0.01; 0.7 -0.7 -0.01; 0.7 -0.9 -0.01; 0.9 -0.9 -0.01]; p(7) = patch(mark(:,1), mark(:,2), mark(:,3), 'black'); % Set the object transparency alpha(0.5)
3차원 객체가 생성되면 기기에서 수집한 회전 행렬 데이터를 hgtransform을 사용하여 플롯에 연결합니다.
tfObject = hgtransform('Parent', ax); set(p, 'Parent', tfObject);
Transform 객체를 사용하여 루프에서 기기 데이터를 끌어오고 그 데이터를 사용하여 객체 방향을 업데이트합니다. 기기에서 전송된 회전 행렬 데이터에는 정밀도 손실이 있으며, 이는 행렬 변환 경고를 야기할 수 있습니다. 이 예제에서는 경고를 차단하여 무시합니다. 정확도를 높이려면 "Euler" 또는 "Quaternion" 특성 데이터를 사용하고 Robotics System Toolbox™에서 그 데이터를 회전 행렬로 변환하면 됩니다.
warning('off', 'MATLAB:hg:DiceyTransformMatrix'); for loop = 1:100 % Acquire device data data = read(c); % Prepare 4-by-4 transform matrix to plot later transformMatrix = eye(4); % Populate the transform matrix with 9 rotation matrix elements for row = 1:3 for column = 1:3 % Extract the 2 bytes representing the current element in the rotation matrix beginIndex = (row-1)*3 + (column-1); element = data(2*beginIndex + (1:2)); transformMatrix(row, column) = double(typecast(uint8(element), 'int16')) / (2^14); end end % Update plot set(tfObject, 'Matrix', transformMatrix); pause(0.1); end
warning('on', 'MATLAB:hg:DiceyTransformMatrix');
기기 연결 닫기
기기 객체를 다 사용했으면 이 객체를 지웁니다.
clear b
