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

Скрипт Open Live Script

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

Настройка оборудования

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

Узнайте и свяжите с устройством

Во-первых, проверяйте, что Bluetooth Низкое энергетическое устройство поддерживает связи путем нахождения его в MATLAB. blelist функционируйте сканирует соседний Bluetooth Низкие энергетические периферийные устройства, которые дают объявление.

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 объект. Устройство имеет "Сервисный сервис" Движения, который содержит "Матричную характеристику" Вращения.

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"  ]}
      ⋮

Считайте данные о датчике

Затем создайте объект для "Матричной характеристики" Вращения путем определения ее сервиса и характеристической информации.

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

Согласно скандинавской документации 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 объект, представляющий скандинавское устройство 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 объект, вытяните данные об устройстве в цикле и используйте данные, чтобы обновить объектную ориентацию. Матричные данные о вращении, отправленные от устройства, имеют потерю точности, которая может вызвать предупреждения матричного преобразования. В данном примере проигнорируйте предупреждение путем подавления его. Для большей точности можно использовать данные о характеристике "Эйлера" или "Кватерниона" и преобразовать его в матрицу вращения использование 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

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