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
    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