M5Stack Basic/Gray/Core2 BLE Active Scan sample

ble-scan-example.ino

#ifndef ARDUINO_M5STACK_Core2
#include <M5Stack.h>
#else
#include <M5Core2.h>
#endif

#include <utility/M5Timer.h>

#include <esp_bt.h>
#include <esp_bt_main.h>
#include <esp_gap_ble_api.h>

#include <string>
#include <array>

#define _ ESP_ERROR_CHECK

//--+----1----+----2----+----3----+----4----+----5----+----6----+----7----+----8----+----9----+----0----+----1----+----2

namespace ble {
  namespace scanner {

    using ScanResult = esp_ble_gap_cb_param_t::ble_scan_result_evt_param;

    using StartedCallback = void (*)();
    using FoundCallback = void (*)(const ScanResult& result);
    using StoppedCallback = void (*)(const int32_t count);

    namespace {
      volatile int32_t counter = 0;
      volatile bool running = false;

      int32_t scanDuration;

      StartedCallback handleStarted = nullptr;
      FoundCallback handleFound = nullptr;
      StoppedCallback handleStopped = nullptr;
    }

    inline void handleEvent(esp_gap_ble_cb_event_t event, esp_ble_gap_cb_param_t *param);

    inline void initBLEController() {
#ifdef ARDUINO_ARCH_ESP32
      if (!btStart())
      {
        _(ESP_FAIL);
      }
#else
      _(nvs_flash_init());

      esp_bt_controller_mem_release(ESP_BT_MODE_CLASSIC_BT);

      esp_bt_controller_config_t config = BT_CONTROLLER_INIT_CONFIG_DEFAULT();
      _(esp_bt_controller_init(&config));
      _(esp_bt_controller_enable(ESP_BT_MODE_BLE));
#endif
    }

    inline void initBLEStack() {
      _(esp_bluedroid_init());
      _(esp_bluedroid_enable());
    }

    inline void begin(StartedCallback onStarted, FoundCallback onFound, StoppedCallback onStopped) {
      initBLEController();
      initBLEStack();
      handleStarted = onStarted;
      handleFound = onFound;
      handleStopped = onStopped;
      _(esp_ble_gap_register_callback(handleEvent));
    }

    inline bool isRunning() {
      return running;
    }

    inline int32_t detected() {
      return counter;
    }

    inline void stop() {
      esp_ble_gap_stop_scanning();
    }

    inline void clear() {
      counter = 0;
    }

    inline void discover(int32_t duration) {

      esp_ble_scan_params_t params {
        .scan_type = BLE_SCAN_TYPE_ACTIVE, // BLE_SCAN_TYPE_PASSIVE
        .own_addr_type = BLE_ADDR_TYPE_PUBLIC,
        .scan_filter_policy = BLE_SCAN_FILTER_ALLOW_ALL, // BLE_SCAN_FILTER_ALLOW_ONLY_WLST,
        .scan_interval = 40 * 16 / 10,
        .scan_window = 30 * 16 / 10,
        .scan_duplicate = BLE_SCAN_DUPLICATE_ENABLE, // BLE_SCAN_DUPLICATE_DISABLE
      };

      stop();
      clear();

      scanDuration = duration;
      _(esp_ble_gap_set_scan_params(&params)); // → ESP_GAP_BLE_SCAN_PARAM_SET_COMPLETE_EVT
    }

    inline void handleScanEvent(esp_gap_ble_cb_event_t event, ScanResult& p) {

      switch (p.search_evt) {

        case ESP_GAP_SEARCH_INQ_RES_EVT:
          ++counter;
          if (handleFound) {
            handleFound(p);
          }
          break;

        case ESP_GAP_SEARCH_INQ_CMPL_EVT:
          running = false;
          if (handleStopped) {
            handleStopped(counter);
          }
          break;

        default:
          break;
      }

    }

    inline void handleEvent(esp_gap_ble_cb_event_t event, esp_ble_gap_cb_param_t* param) {
      switch (event) {

        case ESP_GAP_BLE_SCAN_PARAM_SET_COMPLETE_EVT:
          {
            _(esp_ble_gap_start_scanning(scanDuration / 1000)); // seconds
            running = true;
            if (handleStarted) {
              handleStarted();
            }
          }
          break;

        case ESP_GAP_BLE_SCAN_RESULT_EVT:
          handleScanEvent(event, param->scan_rst);
          break;

        default:
          break;

      }
    }

  };

}

//--+----1----+----2----+----3----+----4----+----5----+----6----+----7----+----8----+----9----+----0----+----1----+----2

namespace {

  M5Timer timer;
  TFT_eSprite offscreen { &M5.Lcd };

  // esp_ble_addr_type_t
  constexpr const std::array<const char*, 4> addrTypes {
    "- -", // BLE_ADDR_TYPE_PUBLIC
    "- R", // BLE_ADDR_TYPE_RANDOM
    "P -", // BLE_ADDR_TYPE_RPA_PUBLIC
    "P R", // BLE_ADDR_TYPE_RPA_RANDOM
  };

  // esp_ble_evt_type_t
  constexpr const std::array<const char*, 5> advTypes {
    "C S", // ESP_BLE_EVT_CONN_ADV (ADV_IND)
    "C -", // ESP_BLE_EVT_CONN_DIR_ADV (ADV_DIRECT_IND)
    "- S", // ESP_BLE_EVT_DISC_ADV (ADV_SCAN_IND)
    "- -", // ESP_BLE_EVT_NON_CONN_ADV (ADV_NONCONN_IND)
    "RSP", // ESP_BLE_EVT_SCAN_RSP
  };


  constexpr int32_t BLE_ADDR_STRING_LENGTH = 17; // xx:xx:xx:xx:xx:xx

  inline std::string formatAddress(const uint8_t* address) {
    char buffer[BLE_ADDR_STRING_LENGTH + 1];
    /*std::*/snprintf(buffer, BLE_ADDR_STRING_LENGTH + 1, "%02x:%02x:%02x:%02x:%02x:%02x", address[0], address[1], address[2], address[3], address[4], address[5]);
    return std::string(buffer);
  }

  inline void dumpAdvertisingData(const uint8_t* p, uint8_t size) {
    auto end = p + size;
    while (p < end) {

      auto length = *p++;
      if (length == 0) {
        break;
      }
      --length;
      auto type = *p++;

      switch (type) {
        case 0x01: // Flags
          Serial.print("[Flags: ");
          for (auto i = 0; i < length; ++i) {
            Serial.printf("%02x", *p++);
          }
          break;
        case 0x02: // Incomplete list of 16-bit Service Class UUIDs
        case 0x03: // Complete list of 16-bit Service Class UUIDs
          Serial.printf("[%s: ", type == 0x02 ? "UUID" : "UUID+");
          for (auto i = 0; i < length; ++i) {
            if (i && i % 2 == 0) {
              Serial.printf(" ");
            }
            Serial.printf("%02x", *p++);
          }
          break;
        case 0x04: // Incomplete list of 32-bit Service Class UUIDs
        case 0x05: // Complete list of 32-bit Service Class UUIDs
          Serial.printf("[%s: ", type == 0x04 ? "UUID" : "UUID+");
          for (auto i = 0; i < length; ++i) {
            if (i && i % 4 == 0) {
              Serial.printf(" ");
            }
            Serial.printf("%02x", *p++);
          }
          break;
        case 0x06: // Incomplete list of 128-bit Service Class UUIDs
        case 0x07: // Complete list of 128-bit Service Class UUIDs
          Serial.printf("[%s: ", type == 0x06 ? "UUID" : "UUID+");
          for (auto i = 0; i < length; ++i) {
            if (i && i % 8 == 0) {
              Serial.printf(" ");
            }
            Serial.printf("%02x", *p++);
          }
          break;
        case 0x08: // Shortened Local Name
          Serial.print("[Short name: ");
          for (auto i = 0; i < length; ++i) {
            Serial.printf("%c", *p++);
          }
          break;
        case 0x09: // Complete Local Name
          Serial.print("[Complete name: ");
          for (auto i = 0; i < length; ++i) {
            Serial.printf("%c", *p++);
          }
          break;
        case 0x24: // URI
          Serial.print("[URI: ");
          for (auto i = 0; i < length; ++i) {
            Serial.printf("%c", *p++);
          }
          break;
        case 0x0a: // Tx Power Level
          Serial.print("[Tx: ");
          Serial.printf("%d", *reinterpret_cast<const int8_t*>(p++));
          for (auto i = 1; i < length; ++i) {
            if (i == 1) {
              Serial.printf(" ");
            }
            Serial.printf("%02x", *p++);
          }
          break;
        case 0xff: // Manufacturer Specific data
          Serial.print("[Manufacturer: ");
          for (auto i = 0; i < length; ++i) {
            if (i == 2) {
              Serial.print(" ");
            }
            Serial.printf("%02x", *p++);
          }
          break;
        default:
          Serial.printf("[%02x: ", type);
          for (auto i = 0; i < length; ++i) {
            Serial.printf("%02x", *p++);
          }
          break;
      }

      Serial.print("]");
    }

  }

  inline void onFound(const esp_ble_gap_cb_param_t::ble_scan_result_evt_param& p) {

    //    BLE address    PDU Adv RSSI Flags Advertising Data / Extended Inquiry Response
    // ----------------- --- --- ---- ----- ---------------------------------------------------------------------------------

    Serial.printf(
      "%s %s %s %4d %c%c%c%c%c [%2d/%2d]",
      // p.search_evt: ESP_GAP_SEARCH_INQ_RES_EVT
      formatAddress(p.bda).c_str(),
      // p.dev_type: ESP_BT_DEVICE_TYPE_BLE
      addrTypes.at(p.ble_addr_type),
      advTypes.at(p.ble_evt_type),
      p.rssi,
      p.flag & ESP_BLE_ADV_FLAG_LIMIT_DISC ? 'L' : '-',
      p.flag & ESP_BLE_ADV_FLAG_GEN_DISC ? 'G' : '-',
      p.flag & ESP_BLE_ADV_FLAG_BREDR_NOT_SPT ? 'B' : '-',
      p.flag & ESP_BLE_ADV_FLAG_DMT_CONTROLLER_SPT ? 'C' : '-',
      p.flag & ESP_BLE_ADV_FLAG_DMT_HOST_SPT ? 'H' : '-',
      // p.num_resps: may 1
      // p.num_dis: garbled?
      p.adv_data_len,
      p.scan_rsp_len
    );


    // AD
    if (p.adv_data_len > 0) {
      Serial.print(" AD: ");
      dumpAdvertisingData(p.ble_adv, p.adv_data_len);
    }

    // EIR
    if (p.scan_rsp_len > 0) {
      Serial.print(" EIR: ");
      dumpAdvertisingData(p.ble_adv + p.adv_data_len, p.scan_rsp_len);
    }

    Serial.println();
  }

  inline void onStarted() {
    log_i("BLE Scan begin.");
    Serial.println("");
    Serial.println("   BLE address    PDU Adv RSSI Flags Advertising Data / Extended Inquiry Response");
    Serial.println("----------------- --- --- ---- ----- ---------------------------------------------------------------------------------");
  }

  inline void onStopped(int32_t counter) {
    Serial.println("----------------- --- --- ---- ----- ---------------------------------------------------------------------------------");
    Serial.println("   BLE address    PDU Adv RSSI Flags Advertising Data / Extended Inquiry Response");
    Serial.println("");
    log_i("BLE Scan done, %d device(s) detected.", counter);
  }

  inline void onLcdUpdate() {

    offscreen.fillSprite(BLACK);
    offscreen.setTextColor(WHITE);
    offscreen.setTextSize(2);

    auto running = ble::scanner::isRunning();
    auto detected = ble::scanner::detected();

    offscreen.setCursor(0, 0);
    offscreen.printf("Status: %11s", running ? "Scanning" : "Idle");

    offscreen.setCursor(0, 20);
    offscreen.printf("BD detected: %6d", detected);

    offscreen.pushSprite((M5.Lcd.width() - offscreen.width()) / 2, (M5.Lcd.height() - offscreen.height()) / 2);
  }

}

void setup() {
  M5.begin();
#ifndef ARDUINO_M5STACK_Core2
  M5.Power.begin();
#endif

  if (!offscreen.createSprite(240, 40)) {
    log_e("createSprite failed!");
  }

  ble::scanner::begin(onStarted, onFound, onStopped);

  auto update = [] {
    onLcdUpdate();
  };

  auto discover = [] {
    ble::scanner::discover(25000); // 25s
  };

  timer.setInterval(1000, update); // 1s
  timer.setInterval(30000, discover); // 30s

  update();
  discover();
}

void loop() {
  M5.update();
  timer.run();
  delay(10);
}