Track Orientation of Bluetooth Low Energy Device

Open Live Script

This example shows how to track device orientation with device motion sensor data using Bluetooth® Low Energy communication.

Hardware Setup

This example uses the Nordic Thingy:52™ device. Nordic Thingy:52 is a Bluetooth Low Energy device with a 9-axis motion sensor. This device provides a rich set of sensor data including raw accelerometer, gyroscope, compass, and fused data. This example uses the device calculated rotation matrix to track the device orientation.

Discover and Connect to Device

First, check that the Bluetooth Low Energy device supports connections by finding it in MATLAB®. The blelist function scans nearby Bluetooth Low Energy peripheral devices that are advertising.

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

After the device is found in MATLAB, connect to it by calling ble. Specify the name of the device if it has a unique name, or specify the device address.

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

Access the Characteristics property of the ble object. The device has the "Motion Service" service, which contains the "Rotation Matrix" characteristic.

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

Read Sensor Data

Next, create an object for the "Rotation Matrix" characteristic by specifying its service and characteristic information.

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

Then read the current rotation matrix value from the device.

data = read(c)

data = 1×18

   253    63   255   255    15     1   255   255     0    64     8     0   240   254   247   255   253    63

According to the Nordic Thingy:52 documentation, this raw data contains a 3-by-3 matrix where each element is a 16-bit integer sent in 2 bytes. Each element represents a signed floating point number, composed of 2 sign-and-exponent bits and 14 fraction bits. Interpret the raw data as a rotation matrix.

% 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

Track Device Orientation

To show live tracking of the device orientation, first plot a 3-D object representing the Nordic Thingy:52 device.

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

After the 3-D object is created, link the rotation matrix data acquired from the device to the plot by using hgtransform.

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

With the Transform object, pull device data in a loop and use the data to update the object orientation. The rotation matrix data sent from the device has precision loss, which can cause matrix transformation warnings. For this example, ignore the warning by suppressing it. For greater accuracy, you can use "Euler" or "Quaternion" characteristic data and convert it to a rotation matrix using 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');

Close Device Connection

Clear the device object when you are finished working with it.

clear b