Nicla Sense MEをデータ入力に使う②クオータニオンのデータをBLEで受け取るセントラル

　クオータニオンの出力をするNicla Sense MEのデータを受ける、セントラルをmatlabで作ります。3Dの表示部分はposeplotを使います。

●実数で送ってくる

　クオータニオンのデータ、x、y、z、wは実数です。IEEE754 単精度浮動小数点数の形式：binary32を実数に戻します。

　実数に戻す処理をテキスト処理で記述した事例です。


function  shortF = hex2numS(decimal4)  % IEE754の単精度浮動小数点数の形式：binary32
    dataBig = strcat(num2str(dec2bin( decimal4(1),8)),num2str(dec2bin( decimal4(2),8)) ...
                  ,num2str(dec2bin( decimal4(3),8)),num2str(dec2bin( decimal4(1),8)));
    sign = (-1)^str2num(dataBig(1));        % 符号
    exponentBit = dataBig(2:1:9);
    fractionBit = ("1" + dataBig(10:1:end));
    fraction = bin2dec(fractionBit)/(2^23) ;          % 仮数
    exponent = 2 ^ (bin2dec(exponentBit) -127) ;      % 指数

    shortF = sign * fraction * exponent;
end

function Q = read_IMU(f11, f12, f13, f14)
    x = hex2numS(flip(read(f11))); % little endian->big endian
    y = hex2numS(flip(read(f12)));
    z = hex2numS(flip(read(f13)));
    w = hex2numS(flip(read(f14)));
    Q = [w x y z];
end

　数値で処理をしたプログラムです。


function  binary32 = IEEE754(fourDigit)  % IEEE754の単精度浮動小数点数の形式：binary32  
    dataBigendian = uint32(bitshift(fourDigit(1), 24) + bitshift(fourDigit(2), 16) + bitshift(fourDigit(3), 8) + fourDigit(4));

    sign = power(-1, double(bitget(dataBigendian,32)));
    fraction = 1.0 + double(bitshift(uint32(bitshift(uint32(bitshift(dataBigendian,1)) , 8)), -9)) / 8388608.0;    % 仮数
    exponents = double(2^(double(double(bitshift(bitshift(dataBigendian,1), -24))-127.0)));   % 指数

    binary32 = sign * fraction * exponents;
    %fprintf(" sign %.1f fraction %.4f exponents %.4f ",sign,fraction,exponents);
end

function Q = read_IMU(f11, f12, f13, f14)
    x = IEEE754(flip(read(f11))); % little endian->big endian
    y = IEEE754(flip(read(f12)));
    z = IEEE754(flip(read(f13)));
    w = IEEE754(flip(read(f14)));
    Q = [w x y z];
end

●メイン・プログラム

　クオータニオンの四つのデータを読んできます。順序は、 w、x、y、zです。

　　rotators = read_IMU(f11, f12, f13, f14);

　次のquaternion関数で、a+bi+cj+dkの形式に変換します。

　　quaternion(rotators)　

　plane3.stlは、Windows10に入っていたスペースシャトルの3Dデータを、Webのあったツールでstlファイル形式に変換しました。



clear
scan = blelist("Timeout", 20);
Nicla_address = "9313D63DDE91";  % scanで見つかったアドレスを代入
b = ble(Nicla_address);

% Nicla_BHI260

ServiceUUID    = "F000AA30-0451-4000-B000-000000000000";
BHI260_QuaterX_Characteristic_UUID   =  "F000AA5A-0451-4000-B000-000000000000";
BHI260_QuaterY_Characteristic_UUID   =  "F000AA5B-0451-4000-B000-000000000000";
BHI260_QuaterZ_Characteristic_UUID   =  "F000AA5C-0451-4000-B000-000000000000";
BHI260_QuaterW_Characteristic_UUID   =  "F000AA5D-0451-4000-B000-000000000000";

f11 = characteristic(b, ServiceUUID, BHI260_QuaterX_Characteristic_UUID);
subscribe(f11);  % Notify
f12 = characteristic(b, ServiceUUID, BHI260_QuaterY_Characteristic_UUID);
subscribe(f12);
f13 = characteristic(b, ServiceUUID, BHI260_QuaterZ_Characteristic_UUID);
subscribe(f13);
f14 = characteristic(b, ServiceUUID, BHI260_QuaterW_Characteristic_UUID);
subscribe(f14);

stopTimer = 200;
disp('start');
figure;
tic;
while(toc < stopTimer)
    rotators = read_IMU(f11, f12, f13, f14);
    poseplot(quaternion(rotators), MeshFileName="plane3.stl",scaleFactor=0.3);
    view([50 50 -10]);
end

unsubscribe(f11);
unsubscribe(f12);
unsubscribe(f13);
unsubscribe(f14);
clear b

　実行例です。