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