LoginSignup
8

More than 1 year has passed since last update.

posted at

updated at

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

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

らへんで使っている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++不慣れなためメモリリークしてる可能性もあるので、バグありましたら指摘してもらえると幸いです。

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
What you can do with signing up
8