Main Content

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
    Index      Name         Address        RSSI    Advertisement
    _____    ________    ______________    ____    _____________

      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] 
     15      ""          "52DBAB89F58F"    -91     [1×1 struct] 
     16      ""          "528E12038BD6"    -91     [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
             ServiceName                           ServiceUUID                               CharacteristicName                           CharacteristicUUID                Attributes  
    ______________________________    ______________________________________    ____________________________________________    ______________________________________    ______________

    "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"  ]}
    "Weather Station Service"         "EF680200-9B35-4933-9B10-52FFA9740042"    "Pressure"                                      "EF680202-9B35-4933-9B10-52FFA9740042"    {["Notify"  ]}
    "Weather Station Service"         "EF680200-9B35-4933-9B10-52FFA9740042"    "Humidity"                                      "EF680203-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