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

　[前回](https://qiita.com/jamjam/items/8a1ba500f073658e23ef)、BLEペリフェラルの投げるデータは文字列でした。今度は実数を扱います。
###ペリフェラル
　Nicla Sense MEボードでクオータニオンの四つの値を実数で送る記事です。

　　[Nicla Sense MEをデータ入力に使う①クオータニオンのデータをBLEで出力するペリフェラル](https://www.denshi.club/matlab/arduino/imu/nicla-sense-me1.html)

　プログラムです。

```C
#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で受け取るセントラル](https://www.denshi.club/matlab/arduino/imu/nicla-sense-me2ble.html)


```C
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用に書き換えました。仮数、指数を求めるところは、コンパイラのエラーが出ないように変更しました。

```C
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](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/407635/c9d1e59e-191b-6e55-3403-8e121d285347.png)


　データを送っているArduinoのシリアルモニタの様子です。
![2021-12-01 (1).png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/407635/b8e9bdab-6f21-aa7f-3757-1f9c66182a45.png)