前にnode版を書きましたが、こちらはESP32編になります。
Sesame自体の解析系についてはnode版を参照下さい。
https://qiita.com/odetarou/items/9628d66d4d94290b5f2d
スマートロックをハックしてESP32とのコラボできた。
— ode (@odetarou) August 27, 2019
Bluetooth接続で角度取得してサーボモーターを回すの術！#sesame pic.twitter.com/tFOR7Nm5l3
Felica Lite-SでのSesame開け締めをBLE化!
— ode (@odetarou) September 4, 2019
全体は早いけどRestのほうが状態取得はクラウドで保持なのか早いので音声を先に流せるから体験はいいような。UIの話っぽい。
ただRestだと最新状態が反映されてないときがあってやり直しになることがあるので、BLE版はそこは確実かなhttps://t.co/LiXEuJ3Uwu pic.twitter.com/a7fxanTYRN
らへんで使っている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++不慣れなためメモリリークしてる可能性もあるので、バグありましたら指摘してもらえると幸いです。