LoginSignup
4
1

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 3 years have passed since last update.

@jamjam(吉田 伸三)

matlab SimulinkのBLE functionを作ってみた　その2　備忘録

Posted at

　前回、BLEペリフェラルの投げるデータは文字列でした。今度は実数を扱います。

ペリフェラル

　Nicla Sense MEボードでクオータニオンの四つの値を実数で送る記事です。

　　Nicla Sense MEをデータ入力に使う①クオータニオンのデータをBLEで出力するペリフェラル

　プログラムです。

# include "Arduino.h"
# include "Arduino_BHY2.h"
# include <ArduinoBLE.h>

SensorQuaternion   quater(SENSOR_ID_RV);

// BLE Service
# define BHI260_SERVICE1_UUID   "F000AA30-0451-4000-B000-000000000000"
BLEService Sensor_BHI260_Service1(BHI260_SERVICE1_UUID);

// BLE  Characteristic
# define BHI260_QuaterX_Characteristic_UUID      "F000AA5A-0451-4000-B000-000000000000"
# define BHI260_QuaterY_Characteristic_UUID      "F000AA5B-0451-4000-B000-000000000000"
# define BHI260_QuaterZ_Characteristic_UUID      "F000AA5C-0451-4000-B000-000000000000"
# define BHI260_QuaterW_Characteristic_UUID      "F000AA5D-0451-4000-B000-000000000000"
BLEFloatCharacteristic    BHI260_QuaternionX(BHI260_QuaterX_Characteristic_UUID, BLERead | BLENotify);
BLEFloatCharacteristic    BHI260_QuaternionY(BHI260_QuaterY_Characteristic_UUID, BLERead | BLENotify);
BLEFloatCharacteristic    BHI260_QuaternionZ(BHI260_QuaterZ_Characteristic_UUID, BLERead | BLENotify);
BLEFloatCharacteristic    BHI260_QuaternionW(BHI260_QuaterW_Characteristic_UUID, BLERead | BLENotify);

// BLE  Descriptor
# define BHI260_Quater_Descriptor_UUID       "2901"
BLEDescriptor   BHI260_Quater_Descriptor(BHI260_Quater_Descriptor_UUID, "Quaternion  x,y,z,w  IEEE754 binary32");

# define localNAME  "Nicla_BHI260"
# define DeviceNAME "NiclaBLE"

float previousMillis = 0;  // last time value was checked, in ms

void setup(){
  Serial.begin(115200);
  while(!Serial);

  BHY2.begin();
  quater.begin();

  if (!BLE.begin()) {
    Serial.println("starting BLE failed!");
    while (1);
  }

  BLE.setLocalName(localNAME);
  BLE.setDeviceName(DeviceNAME);

  //// set the service
  BLE.setAdvertisedService(Sensor_BHI260_Service1);

  // add characteristic
  Sensor_BHI260_Service1.addCharacteristic(BHI260_QuaternionX);
  Sensor_BHI260_Service1.addCharacteristic(BHI260_QuaternionY);
  Sensor_BHI260_Service1.addCharacteristic(BHI260_QuaternionZ);
  Sensor_BHI260_Service1.addCharacteristic(BHI260_QuaternionW);
  
  // add descriptor
  BHI260_QuaternionX.addDescriptor(BHI260_Quater_Descriptor);

  // add service
  BLE.addService(Sensor_BHI260_Service1);

  // set initial value for this characteristic ; PASS

  // start advertising
  BLE.advertise();
  Serial.println("\nBluetooth device active, waiting for connections...");
}

void loop(){
  // wait for a BLE central
  BLEDevice central = BLE.central();

  // if a central is connected to the peripheral:
  if (central) {
    delay(100);
    Serial.print("\nConnected to central: ");
    // print the central's BT address:
    Serial.println(central.address());

    // check data every 200ms
    // while the central is connected:
    while (central.connected()) {
      long currentMillis = millis();
      // if 200ms have passed, check value:
      if (currentMillis - previousMillis >= 200) {
        previousMillis = currentMillis;
        updateValue();
        //delay(1000);
      }
    }
    // when the central disconnects
    Serial.print("Disconnected from central: ");
    Serial.println(central.address());
    goto brout;
  }
  brout: ;
}

void updateValue() {
  BHY2.update();
  delay(500);
  Serial.println(String("Quaternion : x,y,z,w ") + String(quater.x(),3) + ", " +  String(quater.y(),3) + ", " + String(quater.z(),3)+ ", "+ String(quater.w(),3));
  BHI260_QuaternionX.writeValue(quater.x());
  BHI260_QuaternionY.writeValue(quater.y());
  BHI260_QuaternionZ.writeValue(quater.z());
  BHI260_QuaternionW.writeValue(quater.w());
}

matlabのセントラル

　次の記事の、実数に戻す処理を数値で記述したプログラムを利用します。

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

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

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

Simulinkのfunction

　function用に書き換えました。仮数、指数を求めるところは、コンパイラのエラーが出ないように変更しました。

function quater = fcn()
    % automatically initialized to []
    persistent b;
    persistent f11;
    persistent f12;
    persistent f13;
    persistent f14;
    %persistent quater;
    % extrinsic declarations
    coder.extrinsic('ble');
    coder.extrinsic('characteristic');
    coder.extrinsic('read');
    coder.extrinsic('subscribe'); 

    if isempty(b)
        Nicla_address = "9313D63DDE91";  % scanで見つかったアドレスを代入
        b = ble(Nicla_address);
        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);
    end
    % initialize output
    %quater = [0 0 0 0];
    quater = read_IMU(f11, f12, f13, f14)';
end

function  binary32 = IEEE754(fourDigit)  % IEEE754の単精度浮動小数点数の形式：binary32  
    coder.extrinsic('bitshift'); 
    coder.extrinsic('bitget'); 
    coder.extrinsic('bitand');
    D4 = 0;
    D3 = 0;
    D2 = 0;
    D1 = 0;
    D4 = bitshift(double(fourDigit(1)), 24);
    D3 = bitshift(double(fourDigit(2)), 16);
    D2 = bitshift(double(fourDigit(3)), 8);
    D1 = double(fourDigit(4));
    %dataBigendian = 0;
    dataBigendian = double( D4+ D3 + D2 + D1 );

    S0 = 0;
    S0 = bitget(dataBigendian,32);
    %sign =0 ;
    sign = power(-1, double(S0));

    F9 = 0;
    F9 = bitand(dataBigendian, double(0x7FFFFF));
    %fraction = 0;
    fraction = 1.0 + F9 / 8388608.0;    % 仮数

    E2 = 0;
    E23 = 0;
    E2= bitand(dataBigendian, double(0x7F800000));
    E23 = bitshift(E2,-23);
    exponents = double(2^(double(E23-127.0)));   % 指数

    binary32 = sign * fraction * exponents;
end

function Q = read_IMU(f11, f12, f13, f14)
    coder.extrinsic('read');
    coder.extrinsic('flip');
    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です。
2021-12-01 (2).png

　データを送っているArduinoのシリアルモニタの様子です。
2021-12-01 (1).png

4
1
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
4
1

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?