M5StickCで接触確認アプリを検出

• 「M5 Atom MatrixとM5StickCで新型コロナウイルス接触確認アプリが周囲に何個あるか数えてみる」の記事を元に手を加えてみました。
• デバイスアドレスと強度(RSSI)を、強度順にソートして表示するようにしました。

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

int scanTime = 4;
BLEScan* pBLEScan;

// 接触確認アプリのUUID
const char* uuid = "0000fd6f-0000-1000-8000-00805f9b34fb";

int deviceNum = 0;
# define LISTDEVMAX 7
typedef struct DEVICE {
    int rssi;
    String addr;
};
DEVICE devices[LISTDEVMAX];

int pivot(int i, int j) {
    int k = i + 1;
    while (k <= j && devices[i].rssi == devices[k].rssi) k++;
    if (k > j) return -1;
    if (devices[i].rssi <= devices[k].rssi) return i;
    return k;
}
int partition(int i, int j, int x) {
    int l = i, r = j;
    while (l <= r) {
        while (l <= j && devices[l].rssi > x)  l++;
        while (r >= i && devices[r].rssi <= x) r--;
        if (l > r) break;
        String addr = devices[l].addr;
        int    rssi = devices[l].rssi;
        devices[l].addr = devices[r].addr;
        devices[l].rssi = devices[r].rssi;
        devices[r].addr = addr;
        devices[r].rssi = rssi;
        l++; r--;
    }
    return l;
}
void quickSort(int i, int j) {
    if (i == j) return;
    int p = pivot(i, j);
    if (p != -1) {
        int k = partition(i, j, devices[p].rssi);
        quickSort(i, k - 1);
        quickSort(k, j);
    }
}

class MyAdvertisedDeviceCallbacks: public BLEAdvertisedDeviceCallbacks {
    void onResult(BLEAdvertisedDevice advertisedDevice) {
       if(advertisedDevice.haveServiceUUID()){
            if(strncmp(advertisedDevice.getServiceUUID().toString().c_str(),uuid, 36) == 0){
                if (deviceNum<LISTDEVMAX){
                  devices[deviceNum].rssi = advertisedDevice.getRSSI();
                  devices[deviceNum].addr = advertisedDevice.getAddress().toString().c_str();
                }
                deviceNum++;
                Serial.print("RSSI: ");
                Serial.println(devices[deviceNum].rssi);
                Serial.print("ADDR: ");
                Serial.println(devices[deviceNum].addr);
                Serial.println("Found!");
            }
        }
    }
};

void drawScreen() {
    M5.Lcd.fillScreen(BLACK);
    M5.Lcd.setTextSize(2);
    M5.Lcd.setCursor(0, 0);
    M5.Lcd.setTextColor(WHITE);
    M5.Lcd.printf("COCOAs: %2d \n", deviceNum);
    M5.Lcd.setTextSize(1);
    M5.Lcd.setTextColor(RED);
    M5.Lcd.printf(" Bat:%5.1fV Chg:%5.1f\n", M5.Axp.GetBatVoltage(), M5.Axp.GetBatCurrent());
    M5.Lcd.setTextColor(BLUE);
    quickSort(0, deviceNum - 1);
    for(int i=0; i<deviceNum; i++){
      M5.Lcd.printf(" %s %03d \n", devices[i].addr.c_str(), devices[i].rssi);
    }
    for(int i=deviceNum; i<LISTDEVMAX; i++){
      M5.Lcd.print("                         \n");
    }
}

void Task1(void *pvParameters) {
  // loop()書くとBLEスキャン中M5.update()が実行されなくてボタンが取れないのでマルチスレッド化している
  while(1) {
    deviceNum = 0;
    BLEScanResults foundDevices = pBLEScan->start(scanTime, false);
    Serial.print("Devices found: ");
    Serial.println(deviceNum);
    Serial.println("Scan done!");
    pBLEScan->clearResults();   // delete results fromBLEScan buffer to release memory
    drawScreen();
  }
}

void setup() {
  M5.begin();
  Serial.begin(115200);
  Serial.println("Scanning...");
  M5.Lcd.setRotation(1);
  M5.Axp.ScreenBreath(8);
  M5.Lcd.fillScreen(BLACK);

  BLEDevice::init("");
  pBLEScan = BLEDevice::getScan(); //create new scan
  pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks());
  pBLEScan->setActiveScan(true); //active scan uses more power, but get results faster
  pBLEScan->setInterval(5000); // スキャン間隔5秒
  pBLEScan->setWindow(4999);  // less or equal setInterval value
  xTaskCreatePinnedToCore(Task1,"Task1", 4096, NULL, 3, NULL, 1);
}

void loop() {
  M5.update();
    if ( M5.BtnA.wasReleased() ) {
      M5.Axp.PowerOff();
  }
}

• 自分のスマホ近づけてもRSSIが-70程度までしか行かない…センサー壊れてる？
• BLEの仕組みを理解していないけど、デバイスアドレスは定期的に変更しているのか、たまに1台が2台分検出されてる？