Отслеживайте ориентацию устройства с низкой энергией Bluetooth

Открыть Live Script

Этот пример показывает, как отслеживать ориентацию устройства с помощью данных датчика движения устройства с помощью связи Bluetooth ® Low Energy.

Setup оборудования

В этом примере используется устройство Nordic Thingy:52™. Nordic Thingy:52 - устройство Bluetooth Low Energy с 9-осевым датчиком движения. Это устройство обеспечивает богатый набор данных о датчике, включая необработанный акселерометр, гироскоп, компас и сплавленные данные. Этот пример использует вычисленную матрицу поворота устройства, чтобы отслеживать ориентацию устройства.

Обнаружение и подключение к устройству

Во-первых, проверьте, что устройство Bluetooth Low Energy поддерживает подключения, найдя его в MATLAB. The 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

Доступ к свойству Characteristics ble объект. Устройство имеет сервис «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, где каждый элемент является 16-битным целым числом, отправленным в 2 байтах. Каждый элемент представляет число со знаком с плавающей точкой, состоящее из 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

Отслеживайте ориентацию устройства

Чтобы показать отслеживание ориентации устройства в реальном времени, сначала постройте график объекта 3-D, представляющего устройство Nordic Thingy:52.

% 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-D соедините данные матрицы поворота, полученные с устройства, с графиком при помощи hgtransform.

tfObject = hgtransform('Parent', ax);
set(p, 'Parent', tfObject);

С Transform объект, извлеките данные устройства в цикл и используйте данные для обновления ориентации объекта. Матричные данные вращения, отправленные с устройства, имеют потерю точности, которая может вызвать предупреждения преобразования матрицы. В данном примере игнорируйте предупреждение, подавив его. Для большей точности можно использовать характеристические данные «Euler» или «Quaternion» и преобразовать их в матрицу вращения с помощью Toolbox™ Robotics System.

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

Документация