BLEによる屋内測位をiBeaconとM5MtickCでやってみた

まずは簡単に測位

以下に記載されているコードでそのまま試してみた。
https://tech.fusic.co.jp/posts/2020-02-23-m5stickc-ibeacon/

※動いているかどうかの確認するために画面出力だけ追記

#include <M5StickC.h>
#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEScan.h>
#include <BLEAdvertisedDevice.h>

class IBeaconAdvertised: public BLEAdvertisedDeviceCallbacks {
  public:
    // BLE検出時のコールバック
    void onResult(BLEAdvertisedDevice device) {
      if (!isIBeacon(device)) {
        return;
      }
      printIBeacon(device);
    }

  private:
    // iBeaconパケット判定
    bool isIBeacon(BLEAdvertisedDevice device) {
      if (device.getManufacturerData().length() < 25) {
        return false;
      }
      if (getCompanyId(device) != 0x004C) {
        return false;
      }
      if (getIBeaconHeader(device) != 0x1502) {
        return false;
      }
      return true;
    }

    // CompanyId取得
    unsigned short getCompanyId(BLEAdvertisedDevice device) {
      const unsigned short* pCompanyId = (const unsigned short*)&device
                                         .getManufacturerData()
                                         .c_str()[0];
      return *pCompanyId;
    }

    // iBeacon Header取得
    unsigned short getIBeaconHeader(BLEAdvertisedDevice device) {
      const unsigned short* pHeader = (const unsigned short*)&device
                                      .getManufacturerData()
                                      .c_str()[2];
      return *pHeader;
    }

    // UUID取得
    String getUuid(BLEAdvertisedDevice device) {
      const char* pUuid = &device.getManufacturerData().c_str()[4];
      char uuid[64] = {0};
      sprintf(
        uuid,
        "%02X%02X%02X%02X-%02X%02X-%02X%02X-%02X%02X-%02X%02X%02X%02X%02X%02X",
        pUuid[0], pUuid[1], pUuid[2], pUuid[3], pUuid[4], pUuid[5], pUuid[6], pUuid[7],
        pUuid[8], pUuid[9], pUuid[10], pUuid[11], pUuid[12], pUuid[13], pUuid[14], pUuid[15]
      );
      return String(uuid);
    }

    // TxPower取得
    signed char getTxPower(BLEAdvertisedDevice device) {
      const signed char* pTxPower = (const signed char*)&device
                                    .getManufacturerData()
                                    .c_str()[24];
      return *pTxPower;
    }

    // iBeaconの情報をシリアル出力
    void printIBeacon(BLEAdvertisedDevice device) {
      Serial.printf("addr:%s rssi:%d uuid:%s power:%d\r\n",
                    device.getAddress().toString().c_str(),
                    device.getRSSI(),
                    getUuid(device).c_str(),
                    *(signed char*)&device.getManufacturerData().c_str()[24]);
    }
};

void setup() {
  M5.begin();
  Serial.begin(115200);
  BLEDevice::init("");
  M5.Lcd.setCursor(0, 30);
  M5.Lcd.print("Ready");
}

void loop() {
  M5.Lcd.print("start.");
  Serial.println("start.");
  BLEScan* scan = BLEDevice::getScan();
  scan->setAdvertisedDeviceCallbacks(new IBeaconAdvertised(), true);
  scan->setActiveScan(true);
  scan->start(60);
  Serial.println("complete.");
  M5.Lcd.print("complete.");
}

結果

こんな感じで簡単に取得。
※iBeaconの発信にはAplixの端末2台を使用。

ここまでやってから、処理や出力の内容を改めて確認してみる。

• addr：BLEの出力側端末（アドバタイズ側）固有の情報
• uuid：サービスUUID固有の情報
• rssi：BLEの受信端末（セントラル）が受信した電波の強度
• power：TxPwoer。BLEの出力側端末が発する信号の強さ。

iBeaconの場合は1m 離れた地点での受信信号強度を指すらしい。
なので、rssiがこのpTxPowerと同じだったら1m離れているよというのがわかる寸法
※遮蔽物がない前提

なんでマイナス値を使っているのかはわからないが、
ざっくり言えばrssiがpowerよりに小さい（マイナス値だから大きい？）場合は1mよりも近くにいるのがわかるということになる。

距離を算出してみる

RSSIとTxPowerから距離を算出することもできるみたいなので、やってみる
参考：https://qiita.com/shu223/items/7c4e87c47eca65724305

d = 10 ^ ((TxPower - RSSI) / 20)

• さきほどの例

addr:c0:1c:4d:42:11:30 rssi:-56 uuid:00000000-9706-1001-B000-001C4D0E77D0 power:-69

TxPower : -69、RSSI : -56なので、以下となる。

d : 10 ^ ((-69 - -56) / 20) = 0.22m

一式PCのそばだったのでこんなものかなという気はする。

• ちょっと離れた状態

addr:c0:1c:4d:42:11:30 rssi:-77 uuid:00000000-9706-1001-B000-001C4D0E77D0 power:-69

TxPower:-69、RSSI:-77なので以下となる。

d : 10 ^ ((-69 - -77) / 20) = 2.51m

目測だがこんなものな気がする。

• M5StickCの上にビーコン端末をのっけた状態

addr:c0:1c:4d:42:1e:74 rssi:-36 uuid:00000000-9706-1001-B000-001C4D0E77D0 power:-69

TxPower:-69、RSSI:-36なので以下となる。

d : 10 ^ ((-69 - -36) / 20) = 0.02m

デバイス同士は接触しているのでこんなもんだろう。

全部合ってそうなぐらいの数値ではある。すごい。

どう使う？

ただ、これだとSCANした結果がコンソールにドバっと取得されてしまうので、使いにくい。いくつか読むと取得した値を平均化して使うのが正しそう。

現状だと

scan->start(60);

これが60秒間スキャンしっぱなしということのようなので、
なので、これを1秒とかの短い時間にし、その中での平均をとるとかがして近い/遠いやその結果の精度を出すとかをするのがよさそう。

どうやらこの辺はiOSのフレームワークだと勝手にやってくれるらしい。

上記以外にもiOSのフレームワークとしては以下も持っているらしい

• 近接度（Proximity）
  • CLProximityImmediate （非常に近い）
  • CLProximityNear （近い）
  • CLProximityFar （遠い）
• accuracy（精度）
  • 推定距離のばらつきを示す値。どれくらい確からしいかを表す値

参考：https://qiita.com/_daisuke0802/items/765757b55e203db0a4b0

なので、次回はこの辺をどうにかしてみたいところ。