Bluetooth Low Energy デバイスの向きの追跡
この例では、Bluetooth® Low Energy 通信を使用して、デバイスのモーション センサー データでデバイスの向きを追跡する方法を説明します。
ハードウェア セットアップ
この例では Nordic Thingy:52™ デバイスを使用します。Nordic Thingy:52 は、9 軸のモーション センサーをもつ Bluetooth Low Energy デバイスです。このデバイスは、加速度計、ジャイロスコープ、コンパスの生データや融合データを含む、豊富なセンサー データのセットを提供します。この例では、デバイスにより計算された回転行列を使用してデバイスの向きを追跡します。
デバイスの検出と接続
最初に、Bluetooth Low Energy デバイスを MATLAB® で検索することにより、デバイスが接続をサポートすることを確認します。関数 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
