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

#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();
  }
}