Отслеживайте ориентацию устройства с низкой энергией Bluetooth
Этот пример показывает, как отслеживать ориентацию устройства с помощью данных датчика движения устройства с помощью связи 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