スマートロックのSesame miniをBluetooth連携する(ESP32編)

前にnode版を書きましたが、こちらはESP32編になります。
Sesame自体の解析系についてはnode版を参照下さい。
https://qiita.com/odetarou/items/9628d66d4d94290b5f2d

スマートロックをハックしてESP32とのコラボできた。
Bluetooth接続で角度取得してサーボモーターを回すの術！#sesame pic.twitter.com/tFOR7Nm5l3

— ode (@odetarou) August 27, 2019

Felica Lite-SでのSesame開け締めをBLE化!
全体は早いけどRestのほうが状態取得はクラウドで保持なのか早いので音声を先に流せるから体験はいいような。UIの話っぽい。
ただRestだと最新状態が反映されてないときがあってやり直しになることがあるので、BLE版はそこは確実かなhttps://t.co/LiXEuJ3Uwu pic.twitter.com/a7fxanTYRN

— ode (@odetarou) September 4, 2019

らへんで使っているSesameとのBluetooth連携部分についてになります。

node版と比べてのESP32ならではのメリット

• ESP32本体がラズパイより安いaliexpressで700円ほど
• 省エネ、起動が瞬間、小さくてコンパクト、豊富なGPIOでのセンサー連携

デメリット

• C++がつらい。
• コンパイルして転送時間がかかるのでnodeよりDX(Developer Experience)が悪い。

Sesameでの未知の動作をハック試行錯誤するならnodeで行い、解析終了後にESP32への移植が良さそうです。

Bluetoothで接続するメリット、Config値の用意

node編に記載しているので参照下さい
https://qiita.com/odetarou/items/9628d66d4d94290b5f2d

用意するもの

• Sesame mini(Sesameでも動くと思います)
• ESP32
• root化済みAndroid携帯

実行方法

コードの最初のほうにあるconfig値を設定し
ESP32へ転送してして下さい。

address値はBLE系のツールでSesameのMacアドレスを確認して入力して下さい。
空のままで実行すると近くのBle端末をScanして自動的に繋がりますが時間がかかる&見つからない場合があるためaddress値を指定するのがおすすめです。

接続後はArduinoのシリアルモニターのシリアル入力で下記コマンドを入力可能です

• "lock" ロックします
• "unlock" アンロック（解錠）します。
• "connect" 接続が切れている場合の再接続に使用します。コマンドを入力しないと接続後10秒で切れます。コマンド実行後は1分後に切れます。
• "disconnect" 接続を切ります。

Arduinoコード

サーボ連携、Felica連携など応用系は除外したSesameの連携部分に絞り込んだコードになります。

main.cpp

#include "BLEDevice.h"

#include "mbedtls/md.h"
#include <sstream>
#include <iomanip>

// config block start
const std::string userId = "メールアドレス";
const std::string password = "Android公式アプリ内からハックして取得するパスワード";
const uint16_t lockMinAngle = 10;
const uint16_t lockMaxAngle = 270;

// 下記はoption。指定したほうがscanがskipできるため早くなる。
const std::string address = "bleのaddress";
const unsigned char manufacturerDataMacData[] = {}; // {0x00,0x00...}のように配列で指定する
// config block end




#define POLLING_INTERVAL 50

class MyAdvertisedDeviceCallbacks: public BLEAdvertisedDeviceCallbacks {
  void onResult(BLEAdvertisedDevice advertisedDevice);
};

class MyClientCallback : public BLEClientCallbacks {
  void onConnect(BLEClient* pclient);
  void onDisconnect(BLEClient* pclient);
};


// The remote service we wish to connect to.
BLEUUID serviceUUID("00001523-1212-efde-1523-785feabcd123");

// The characteristic of the remote service we are interested in.
BLEUUID    charUUIDCmd(         "00001524-1212-efde-1523-785feabcd123");
BLEUUID    charUUIDStatus(      "00001526-1212-efde-1523-785feabcd123");
BLEUUID    charUUIDAngleStatus( "00001525-1212-efde-1523-785feabcd123");


boolean doConnect = false;
boolean connected = false;
boolean doScan = false;
boolean doConnecting = false;
BLEClient*  pClient;
BLERemoteCharacteristic* pRemoteCharacteristic;
BLERemoteCharacteristic* pRemoteCharacteristicStatus;
BLERemoteCharacteristic* pRemoteCharacteristicAngleStatus;
BLEAddress bleAddress("");
std::string manufacturerDataMacDataString;

bool isLock = false;
uint_fast8_t lockStatusSet = 0;

void setup() {
  Serial.begin(115200);
  Serial.println("Starting Arduino BLE Client application...");

  // 内蔵LED初期化
  pinMode(2, OUTPUT);
  
  BLEDevice::init("");

  pClient  = BLEDevice::createClient();
  pClient->setClientCallbacks(new MyClientCallback());

  if (address == "") {
    BLEScan* pBLEScan = BLEDevice::getScan();
    pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks());
    // pBLEScan->setInterval(1349);
    // pBLEScan->setWindow(449);
    // https://electronics.stackexchange.com/questions/82098/ble-scan-interval-and-window
    pBLEScan->setInterval(40);
    pBLEScan->setWindow(30);
    pBLEScan->setActiveScan(true);
    pBLEScan->start(10, false);
  } else {
    bleAddress = BLEAddress(address);
    manufacturerDataMacDataString = std::string(reinterpret_cast< char const* >(manufacturerDataMacData), sizeof(manufacturerDataMacData));
    //connectToServer();
  }
} // End of setup.

// This is the Arduino main loop function.
void loop() {

  // bleサンプルのままのscan後に接続しにいく箇所。直接address指定していれば不要。
  // // If the flag "doConnect" is true then we have scanned for and found the desired
  // // BLE Server with which we wish to connect.  Now we connect to it.  Once we are 
  // // connected we set the connected flag to be true.
  if (doConnect == true) {
    if (connectToServer()) {
      Serial.println("We are now connected to the BLE Server.");
    } else {
      Serial.println("We have failed to connect to the server; there is nothin more we will do.");
    }
    doConnect = false;
  }
  if (connected) {
  } else if(doScan) {
    BLEDevice::getScan()->start(0);  // this is just eample to start scan after disconnect, most likely there is better way to do it in arduino
  }
  
   // serial入力によっての処理。 
   String str = Serial.readStringUntil('\n');
   str.trim();
   if (str != "") {
     Serial.println("serial input: " + str);
   }
   if (str == "lock") {
     lock(1);
   } else if (str == "unlock") {
    lock(2);
   } else if (str == "connect") {
     if (connectToServer()) {
       Serial.println("We are now connected to the BLE Server.");
     } else {
       Serial.println("We have failed to connect to the server; there is nothin more we will do.");
     }
   } else if (str == "disconnect") {
     pClient->disconnect();
   }
    
  // 接続した後にtoggleするサンプル
  if (lockStatusSet == 1) {
    if (isLock) {
      lock(2);
    } else {
      lock(1);
    }

    // ロックした直後に切断する場合はコメントアウトする
    // pClient->disconnect();
    lockStatusSet = 2;
  }

  delay(POLLING_INTERVAL);
} // End of loop


bool connectToServer() {
  // リトライ行う。
  for (int i=0; i < 3; i++) {
    if (connectToServerInner()) {
      return true;
    }
  }
  return false;
}

bool connectToServerInner() {
    Serial.print("Forming a connection to ");
    Serial.println(bleAddress.toString().c_str());
    
    // 再利用されるようにsetupへ移動。再利用されればgetServiceのキャッシュがきくはずなので。
    // pClient  = BLEDevice::createClient();
    // Serial.println(" - Created client");

    // pClient->setClientCallbacks(new MyClientCallback());

    doConnecting = true;

    // Connect to the remove BLE Server.   
    if (!pClient->connect(bleAddress, BLE_ADDR_TYPE_RANDOM)) {
      Serial.println(" - connect failure. return false.");
      return false;
    }
    Serial.println(" - Connected to server");

    if (!doConnecting) {
      return false;
    }

    // Obtain a reference to the service we are after in the remote BLE server.
    BLERemoteService* pRemoteService = pClient->getService(serviceUUID);
    if (pRemoteService == nullptr) {
      Serial.print("Failed to find our service UUID: ");
      Serial.println(serviceUUID.toString().c_str());
      pClient->disconnect();
      return false;
    }
    Serial.println(" - Found our service");

    subscribeStatus(pRemoteService);
    subscribeAngleStatus(pRemoteService);

    // Obtain a reference to the characteristic in the service of the remote BLE server.
    pRemoteCharacteristic = pRemoteService->getCharacteristic(charUUIDCmd);
    if (pRemoteCharacteristic == nullptr) {
      Serial.print("Failed to find our characteristic UUID: ");
      Serial.println(charUUIDCmd.toString().c_str());
      pClient->disconnect();
      return false;
    }
    Serial.println(" - Found our characteristic Cmd");

    // Read the value of the characteristic.
    if(pRemoteCharacteristic->canRead()) {
      std::string value = pRemoteCharacteristic->readValue();
      Serial.print("The characteristic value was: ");
      Serial.println(string_to_hex(value).c_str());
      //Serial.println(value.length()); // cmdだと3
    } else {
      Serial.println(" - characteristic cant't read");
      return false;
    }

    // lock(0)だとErrorUnknownCmdになるが、認証することで現在の角度がわかるため実行する
    lock(0);

    connected = true;
    // 内蔵LEDをONにする
    digitalWrite(2, HIGH);

    return true;
}

void subscribeStatus(BLERemoteService* pRemoteService) {
  
    // Obtain a reference to the characteristic in the service of the remote BLE server.
    pRemoteCharacteristicStatus = pRemoteService->getCharacteristic(charUUIDStatus);
    if (pRemoteCharacteristicStatus == nullptr) {
      Serial.print("Failed to find our characteristic UUID: ");
      Serial.println(charUUIDStatus.toString().c_str());
//      pClient->disconnect();
      //return false;
    }
    Serial.println(" - Found our characteristic status");

    if(pRemoteCharacteristicStatus->canNotify()) {
      pRemoteCharacteristicStatus->registerForNotify(notifyCallbackStatus);
    }
}


void subscribeAngleStatus(BLERemoteService* pRemoteService) {
  
    // Obtain a reference to the characteristic in the service of the remote BLE server.
    pRemoteCharacteristicAngleStatus = pRemoteService->getCharacteristic(charUUIDAngleStatus);
    if (pRemoteCharacteristicAngleStatus == nullptr) {
      Serial.print("Failed to find our characteristic UUID: ");
      Serial.println(charUUIDAngleStatus.toString().c_str());
//      pClient->disconnect();
      //return false;
    }
    Serial.println(" - Found our characteristic angle status");

    if(pRemoteCharacteristicAngleStatus->canNotify()) {
      pRemoteCharacteristicAngleStatus->registerForNotify(notifyCallbackAngleStatus);
    }
}

static void notifyCallbackStatus(
  BLERemoteCharacteristic* pBLERemoteCharacteristic,
  uint8_t* pData,
  size_t length,
  bool isNotify) {
    Serial.print("Notify callback for characteristic status");
    Serial.print(pBLERemoteCharacteristic->getUUID().toString().c_str());
    Serial.print(" of data length ");
    Serial.println(length);

    std::vector<byte> data(pData, pData + length);
    std::string dataString = vector_to_hex(data);

    Serial.print("data: ");
    Serial.println(dataString.c_str());

    uint32_t sn;
    memcpy(&sn, pData+6, 4);
    sn++;
    Serial.printf("sn: %u\n", sn);
    uint8_t err = *(pData+14);
    std::vector<std::string> errMsgs =
    {
      "Timeout",
      "Unsupported",
      "Success",
      "Operating",
      "ErrorDeviceMac",
      "ErrorUserId",
      "ErrorNumber",
      "ErrorSignature",
      "ErrorLevel",
      "ErrorPermission",
      "ErrorLength",
      "ErrorUnknownCmd",
      "ErrorBusy",
      "ErrorEncryption",
      "ErrorFormat",
      "ErrorBattery",
      "ErrorNotSend"
    };
    Serial.printf("status update %s, sn=%d, err=%s\n", pData, sn, errMsgs[err+1].c_str());
}


static void notifyCallbackAngleStatus(
  BLERemoteCharacteristic* pBLERemoteCharacteristic,
  uint8_t* pData,
  size_t length,
  bool isNotify)
{
  // Serial.print("Notify callback for characteristic angle status");
  // Serial.print(pBLERemoteCharacteristic->getUUID().toString().c_str());
  // Serial.print(" of data length ");
  // Serial.println(length);

  std::vector<byte> data(pData, pData + length);
  std::string dataString = vector_to_hex(data);

  // Serial.print("data: ");
  // Serial.println(dataString.c_str());

  uint16_t angleRaw;
  memcpy(&angleRaw, pData+2, 2);
  // Serial.printf("angleRaw = %d\n", angleRaw);
  double xx = (((double)angleRaw)/1024*360);
  // Serial.printf("xx = %lf.\n", xx);
  double angle = std::floor(xx);

  if (angle < lockMinAngle || angle > lockMaxAngle) {
    isLock = true;
  } else {        
    isLock = false;
  }
  Serial.printf("angle = %lf. lockStatus:%s\n", angle, isLock ? "true" : "false");

  // サーボ回すのはここで角度をもとにサーボを回す処理をいれました。

  // 初回ロック状態取得時の処理
  // 当初はここでコールバックメソッドを呼ぶ形にしようとしたが、ここでlockを呼ぶとpRemoteCharacteristic->canNotify()が動かなかった。ここ自体がBLEのコールバック処理なのでその中で新たなBLEのread, write処理はまずいのかもしれない。
  if (lockStatusSet == 0) {
    lockStatusSet = 1;   
  
//    if (isLock) {
//      lock(2);
//    } else {
//      lock(1);
//    }
  }
}

void MyClientCallback::onConnect(BLEClient* pclient) {
  Serial.println("onConnect");
}

void MyClientCallback::onDisconnect(BLEClient* pclient) {
  connected = false;
  doConnecting = false;
  lockStatusSet = 0;
  Serial.println("onDisconnect");
  // 内蔵LEDをOFFにする
  digitalWrite(2, LOW);
}

std::vector<byte> sign(uint8_t code, std::string payload, std::string password, std::string macData, std::string userId, uint32_t nonce) {

  byte md5Result[16];
  byte hmacResult[32];

  //  Serial.printf("macData: %s\n", macData.c_str());
  // Serial.printf("payload: %s, len:%d\n", payload.c_str(), payload.length());

  mbedtls_md_context_t ctx;
  mbedtls_md_type_t md_type = MBEDTLS_MD_MD5;

  mbedtls_md_init(&ctx);
  mbedtls_md_setup(&ctx, mbedtls_md_info_from_type(md_type), 0);
  mbedtls_md_starts(&ctx);
  mbedtls_md_update(&ctx, (const unsigned char *) userId.c_str(), userId.length());
  mbedtls_md_finish(&ctx, md5Result);
  mbedtls_md_free(&ctx);  

  // Serial.print("md5: ");
 
  // for(int i= 0; i< sizeof(md5Result); i++){
  //     char str[3];
 
  //     sprintf(str, "%02x", (int)md5Result[i]);
  //     Serial.print(str);
  // }
  // Serial.println("");

  size_t buf_length = payload.length() + 59;
  byte *buf = new byte[buf_length];
  // macData.copy(buf, 32); // len = 6
  memcpy(buf+32, macData.c_str(), 6);
  memcpy(buf+38, md5Result, 16);
  memcpy(buf+54, (uint32_t *) &nonce, 4);
  memcpy(buf+58, (uint8_t *) &code, 1);
  memcpy(buf+59, payload.c_str(), payload.length());

  // Serial.printf("pass length: %d\n", password.length());
  // Serial.printf("pass: %s\n", password.c_str());
  size_t password_buf_length = password.length()/2;
  unsigned char *password_buf = new unsigned char[password_buf_length];
  fromHex(password, password_buf);

  // Serial.print("pass from byte: ");
  // for(int i= 0; i< password_buf_length; i++){
  //     char str[3];
 
  //     sprintf(str, "%02x", (int)password_buf[i]);
  //     Serial.print(str);
  // }
  // Serial.println("");
  
  size_t buf_hmac_data_length = 59 - 32 + payload.length();
  byte *buf_hmac_data = new byte[buf_hmac_data_length];
  memcpy(buf_hmac_data, buf+32, buf_hmac_data_length);

  // Serial.print("buf_hmac_data: ");
  // for(int i= 0; i< buf_hmac_data_length; i++){
  //     char str[3];
 
  //     sprintf(str, "%02x", (int)buf_hmac_data[i]);
  //     Serial.print(str);
  // }
  // Serial.println("");

  md_type = MBEDTLS_MD_SHA256;
  mbedtls_md_init(&ctx);
  mbedtls_md_setup(&ctx, mbedtls_md_info_from_type(md_type), 1);
  mbedtls_md_hmac_starts(&ctx, (const unsigned char *) password_buf, password_buf_length);
  mbedtls_md_hmac_update(&ctx, (const unsigned char *) buf_hmac_data, buf_hmac_data_length);
  mbedtls_md_hmac_finish(&ctx, hmacResult);
  mbedtls_md_free(&ctx);

  delete[] buf_hmac_data;
  delete[] password_buf;
         
  // Serial.print("Hash: ");
  // for(int i= 0; i< sizeof(hmacResult); i++){
  //     char str[3];
 
  //     sprintf(str, "%02x", (int)hmacResult[i]);
  //     Serial.print(str);
  // }
  // Serial.println("");

  memcpy(buf, hmacResult, 32);

  std::vector<byte> bufVector(buf, buf + buf_length);
  //std::string buf_str((char *)buf); stringだと途中に0が入った場合に終端扱いになりデータが切れるのでvectorにしたが、コンストラクタにlengthを渡せばstringでも大丈夫なのを後ほど知る
  delete[] buf;
  return bufVector;
}


// https://stackoverflow.com/questions/3381614/c-convert-string-to-hexadecimal-and-vice-versa
std::string vector_to_hex(const std::vector<byte>& input)
{
    static const char* const lut = "0123456789ABCDEF";
    size_t len = input.size();

    std::string output;
    output.reserve(2 * len);
    for (size_t i = 0; i < len; ++i)
    {
        const unsigned char c = input[i];
        output.push_back(lut[c >> 4]);
        output.push_back(lut[c & 15]);
    }
    return output;
}

std::string string_to_hex(const std::string& input)
{
    static const char* const lut = "0123456789ABCDEF";
    size_t len = input.size();

    std::string output;
    output.reserve(2 * len);
    for (size_t i = 0; i < len; ++i)
    {
        const unsigned char c = input[i];
        output.push_back(lut[c >> 4]);
        output.push_back(lut[c & 15]);
    }
    return output;
}

// https://tweex.net/post/c-anything-tofrom-a-hex-string/
void fromHex(
    const std::string &in,     //!< Input hex string
    void *const data           //!< Data store
    )
{
    size_t          length    = in.length();
    unsigned char   *byteData = reinterpret_cast<unsigned char*>(data);
    
    std::stringstream hexStringStream; hexStringStream >> std::hex;
    for(size_t strIndex = 0, dataIndex = 0; strIndex < length; ++dataIndex)
    {
        // Read out and convert the string two characters at a time
        const char tmpStr[3] = { in[strIndex++], in[strIndex++], 0 };

        // Reset and fill the string stream
        hexStringStream.clear();
        hexStringStream.str(tmpStr);

        // Do the conversion
        int tmpValue = 0;
        hexStringStream >> tmpValue;
        byteData[dataIndex] = static_cast<unsigned char>(tmpValue);
    }
}

inline uint32_t swap32(uint32_t value)
{
    uint32_t ret;
    ret  = value              << 24;
    ret |= (value&0x0000FF00) <<  8;
    ret |= (value&0x00FF0000) >>  8;
    ret |= value              >> 24;
    return ret;
}


bool lock(uint8_t cmdValue) {
  // Read the value of the characteristic.
  if(pRemoteCharacteristicStatus->canRead()) {
    std::string value = pRemoteCharacteristicStatus->readValue();
    Serial.print("The characteristic value was: ");
    Serial.println(string_to_hex(value).c_str());
    //Serial.println(value.length()); // statusだと15

    std::string substr = value.substr(6,4);
    uint32_t sn = substr[0] + (substr[1] << 8) + (substr[2] << 16) + (substr[3] << 24) + 1;
    //Serial.printf("%d %d %d %d\n", substr[0], substr[1], substr[2], substr[3]);
    Serial.printf("sn: %u\n", sn);

    std::string macData = manufacturerDataMacDataString;

    //Serial.print("macData:");
    //Serial.println(string_to_hex(macData).c_str());
    // Serial.println(macData.length());

    //uint8_t cmdValue = 0;
    //cmdValue = 1; // lock
    //cmdValue = 2; // unlock
    std::vector<byte> payload = sign(cmdValue, std::string(""), password, macData.substr(3), userId, sn);        
    
    Serial.printf("write payload hex: %s\n", vector_to_hex(payload).c_str());

    write(payload);
    return true;
  } else {
    Serial.println(" - characteristic status cant't read");
    return false;
  }
}

void write(std::vector<byte> payload) {

  for(int i=0; i<payload.size(); i+=19) {
    size_t sz = std::min((int) payload.size() - i, 19);
    std::vector<byte> buf(sz + 1);
    if (sz < 19) {
      buf[0] = 4;
    } else if (i == 0) {
      buf[0] = 1;
    } else {
      buf[0] = 2;
    }

    copy(payload.begin()+i, payload.begin()+(i+sz), buf.begin() + 1);
    pRemoteCharacteristic->writeValue(buf.data(), buf.size(), false);
  }
}


/**
 * Scan for BLE servers and find the first one that advertises the service we are looking for.
 */
void MyAdvertisedDeviceCallbacks::onResult(BLEAdvertisedDevice advertisedDevice) {
  Serial.print("BLE Advertised Device found: ");
  Serial.println(advertisedDevice.toString().c_str());

  // We have found a device, let us now see if it contains the service we are looking for.
  if (advertisedDevice.haveServiceUUID() && advertisedDevice.isAdvertisingService(serviceUUID)) {

    BLEDevice::getScan()->stop();

    bleAddress = BLEAddress(advertisedDevice.getAddress());
    manufacturerDataMacDataString = advertisedDevice.getManufacturerData();
    
    doConnect = true;
    doScan = true;

  } // Found our server
} // onResult

コードについて

ESP32のBLE注意点

arduino-esp32のBLEにバグがあったようで下記パッチを適用する必要がありました。

2019/09/10時点でmasterのソースを確認したところ2019/8/20に修正されているようなので、最新版ならパッチあては不要そうです。
https://github.com/espressif/arduino-esp32/commit/a12d609b221ce339de092cfe096c6bef7542c943#diff-5045d5b1ea8d54ddfe1d0f59d6e4f098

それ以前のversionの場合は下記問題があるため
https://github.com/nkolban/esp32-snippets/issues/863
下記行を10から1にする必要があります。
https://github.com/nkolban/esp32-snippets/blob/fe3d318acddf87c6918944f24e8b899d63c816dd/cpp_utils/BLERemoteService.cpp#L129

C、C++で学んだこと

• Cの配列は固定長しかだめで、可変長なのはnew(malloc)&deleteが必要。また割り当てたbyte数を知りたい際にポインタにsizeofしてもポインタ自体のサイズの4byteしか返ってこないため別途lengthは変数定義して保持が必要（つらい）
  • 可変長なデータは文字列はC++のstd::string、リストはstd::vectorにまかせると楽。
• 可変長byte配列の表し方
  • arduino-esp32側でstd::stringでbyte配列を定義している箇所が見受けられたため最初signの戻り値はstd::stringにしたが途中に0のデータが来ると終端文字列扱いでそれ以降のデータが切れる問題が発生。そのためstd::vectorにしたが、後日stringの第2引数に長さを渡せば途中に0のデータも保持できるのを発見。manufacturerDataMacDataStringを初期化する際にはstd::stringを長さ指定で使用しています。

Arduinoでのデバッグtips

ArduinoのメニューのToolでCore Debug LevelをVerboseにするとBLEの詳細な情報がでるので調査時に便利でした。
ただ設定しただけだと出力されなかったためarduino-esp32の下記ソース箇所を変更しました。（もっといい方法がありそうですが）

tools/sdk/include/config/sdkconfig.h
-#define CONFIG_LOG_DEFAULT_LEVEL 1
+#define CONFIG_LOG_DEFAULT_LEVEL 5

tools/sdk/sdkconfig
-CONFIG_LOG_DEFAULT_LEVEL=1
+CONFIG_LOG_DEFAULT_LEVEL=5

動作環境

arduino-esp32 git commit 7dbda4988b60f83ecfc887c7f92233b2fb3f63dd Tue Jul 9 23:13:09 2019 -0700

おわりに

Node → C++化するのにあたりbyte操作多めで不慣れで時間かかりましたが無事ESP32で動いてよかったです。
C++不慣れなためメモリリークしてる可能性もあるので、バグありましたら指摘してもらえると幸いです。