From e464f33da3389c88a03cca212592fa0993af2362 Mon Sep 17 00:00:00 2001 From: Maxim Slipenko Date: Thu, 7 Dec 2023 23:51:43 +0300 Subject: [PATCH] wip --- components/esp32_presense/BleFingerprint.cpp | 553 ++++++++++++++++++ components/esp32_presense/BleFingerprint.h | 160 +++++ .../BleFingerprintCollection.cpp | 260 ++++++++ .../esp32_presense/BleFingerprintCollection.h | 54 ++ .../esp32_presense/FilteredDistance.cpp | 76 +++ components/esp32_presense/FilteredDistance.h | 37 ++ components/esp32_presense/MiFloraHandler.cpp | 137 +++++ components/esp32_presense/MiFloraHandler.h | 15 + .../esp32_presense/NameModelHandler.cpp | 24 + components/esp32_presense/NameModelHandler.h | 11 + components/esp32_presense/QueryReport.h | 17 + components/esp32_presense/__init__.py | 52 ++ components/esp32_presense/esp32_presense.cpp | 81 +++ components/esp32_presense/esp32_presense.h | 37 ++ components/esp32_presense/rssi.h | 19 + components/esp32_presense/string_utils.cpp | 134 +++++ components/esp32_presense/string_utils.h | 20 + components/esp32_presense/util.h | 37 ++ 18 files changed, 1724 insertions(+) create mode 100644 components/esp32_presense/BleFingerprint.cpp create mode 100644 components/esp32_presense/BleFingerprint.h create mode 100644 components/esp32_presense/BleFingerprintCollection.cpp create mode 100644 components/esp32_presense/BleFingerprintCollection.h create mode 100644 components/esp32_presense/FilteredDistance.cpp create mode 100644 components/esp32_presense/FilteredDistance.h create mode 100644 components/esp32_presense/MiFloraHandler.cpp create mode 100644 components/esp32_presense/MiFloraHandler.h create mode 100644 components/esp32_presense/NameModelHandler.cpp create mode 100644 components/esp32_presense/NameModelHandler.h create mode 100644 components/esp32_presense/QueryReport.h create mode 100644 components/esp32_presense/__init__.py create mode 100644 components/esp32_presense/esp32_presense.cpp create mode 100644 components/esp32_presense/esp32_presense.h create mode 100644 components/esp32_presense/rssi.h create mode 100644 components/esp32_presense/string_utils.cpp create mode 100644 components/esp32_presense/string_utils.h create mode 100644 components/esp32_presense/util.h diff --git a/components/esp32_presense/BleFingerprint.cpp b/components/esp32_presense/BleFingerprint.cpp new file mode 100644 index 0000000..8e07551 --- /dev/null +++ b/components/esp32_presense/BleFingerprint.cpp @@ -0,0 +1,553 @@ +#include "BleFingerprint.h" + +#include "MiFloraHandler.h" +#include "NameModelHandler.h" +#include "BleFingerprintCollection.h" +#include "mbedtls/aes.h" +#include "rssi.h" +#include "string_utils.h" +#include "util.h" + +class ClientCallbacks : public BLEClientCallbacks { + bool onConnParamsUpdateRequest(NimBLEClient *pClient, const ble_gap_upd_params *params) { + return true; + }; +}; + +static ClientCallbacks clientCB; + +BleFingerprint::BleFingerprint(BLEAdvertisedDevice *advertisedDevice, float fcmin, float beta, float dcutoff) : filteredDistance{FilteredDistance(fcmin, beta, dcutoff)} { + firstSeenMillis = millis(); + address = NimBLEAddress(advertisedDevice->getAddress()); + addressType = advertisedDevice->getAddressType(); + rssi = advertisedDevice->getRSSI(); + raw = dist = pow(10, float(get1mRssi() - rssi) / (10.0f * BleFingerprintCollection::absorption)); + seenCount = 1; + queryReport = nullptr; + fingerprintAddress(); +} + +void BleFingerprint::setInitial(const BleFingerprint &other) { + rssi = other.rssi; + dist = other.dist; + raw = other.raw; + filteredDistance = other.filteredDistance; +} + +bool BleFingerprint::shouldHide(const std::string &s) { + if (BleFingerprintCollection::include.length() > 0 && !prefixExists(BleFingerprintCollection::include, s)) return true; + return (BleFingerprintCollection::exclude.length() > 0 && prefixExists(BleFingerprintCollection::exclude, s)); +} + +bool BleFingerprint::setId(const std::string &newId, short newIdType, const std::string &newName) { + if (idType < 0 && newIdType < 0 && newIdType >= idType) return false; + if (idType > 0 && newIdType <= idType) return false; + // Serial.printf("setId: %s %d %s OLD idType: %d\r\n", newId.c_str(), newIdType, newName.c_str(), idType); + + ignore = newIdType < 0; + idType = newIdType; + + DeviceConfig dc; + if (BleFingerprintCollection::FindDeviceConfig(newId, dc)) { + if (dc.calRssi != NO_RSSI) + calRssi = dc.calRssi; + if (!dc.alias.isEmpty()) + return setId(dc.alias, ID_TYPE_ALIAS, dc.name); + if (!dc.name.isEmpty()) + name = dc.name; + } else if (!newName.isEmpty() && name != newName) + name = newName; + + if (id != newId) { + bool newHidden = shouldHide(newId); + countable = !ignore && !hidden && !BleFingerprintCollection::countIds.isEmpty() && prefixExists(BleFingerprintCollection::countIds, newId); + bool newQuery = !ignore && !BleFingerprintCollection::query.isEmpty() && prefixExists(BleFingerprintCollection::query, newId); + if (newQuery != allowQuery) { + allowQuery = newQuery; + if (allowQuery) { + qryAttempts = 0; + if (rssi < -80) { + qryDelayMillis = 30000; + lastQryMillis = millis(); + } else if (rssi < -70) { + qryDelayMillis = 5000; + lastQryMillis = millis(); + } + } + } + id = newId; + hidden = newHidden; + added = false; + } + + return true; +} + +const std::string BleFingerprint::getMac() const { + const auto nativeAddress = address.getNative(); + return Sprintf("%02x%02x%02x%02x%02x%02x", nativeAddress[5], nativeAddress[4], nativeAddress[3], nativeAddress[2], nativeAddress[1], nativeAddress[0]); +} + +const int BleFingerprint::get1mRssi() const { + if (calRssi != NO_RSSI) return calRssi + BleFingerprintCollection::rxAdjRssi; + if (bcnRssi != NO_RSSI) return bcnRssi + BleFingerprintCollection::rxAdjRssi; + if (mdRssi != NO_RSSI) return mdRssi + BleFingerprintCollection::rxAdjRssi; + if (asRssi != NO_RSSI) return asRssi + BleFingerprintCollection::rxAdjRssi; + return BleFingerprintCollection::rxRefRssi + DEFAULT_TX + BleFingerprintCollection::rxAdjRssi; +} + +void BleFingerprint::fingerprint(NimBLEAdvertisedDevice *advertisedDevice) { + if (advertisedDevice->haveName()) { + const std::string name = advertisedDevice->getName(); + if (!name.empty()) setId(std::string("name:") + kebabify(name).c_str(), ID_TYPE_NAME, std::string(name.c_str())); + } + + if (advertisedDevice->getAdvType() > 0) + connectable = true; + + size_t serviceAdvCount = advertisedDevice->getServiceUUIDCount(); + size_t serviceDataCount = advertisedDevice->getServiceDataCount(); + bool haveTxPower = advertisedDevice->haveTXPower(); + int8_t txPower = advertisedDevice->getTXPower(); + + if (serviceAdvCount > 0) fingerprintServiceAdvertisements(advertisedDevice, serviceAdvCount, haveTxPower, txPower); + if (serviceDataCount > 0) fingerprintServiceData(advertisedDevice, serviceDataCount, haveTxPower, txPower); + if (advertisedDevice->haveManufacturerData()) fingerprintManufactureData(advertisedDevice, haveTxPower, txPower); +} + +int bt_encrypt_be(const uint8_t *key, const uint8_t *plaintext, uint8_t *enc_data) { + mbedtls_aes_context ctx; + mbedtls_aes_init(&ctx); + + if (mbedtls_aes_setkey_enc(&ctx, key, 128) != 0) { + mbedtls_aes_free(&ctx); + return BLE_HS_EUNKNOWN; + } + + if (mbedtls_aes_crypt_ecb(&ctx, MBEDTLS_AES_ENCRYPT, plaintext, enc_data) != 0) { + mbedtls_aes_free(&ctx); + return BLE_HS_EUNKNOWN; + } + + mbedtls_aes_free(&ctx); + return 0; +} + +struct encryption_block { + uint8_t key[16]; + uint8_t plain_text[16]; + uint8_t cipher_text[16]; +}; + +bool ble_ll_resolv_rpa(const uint8_t *rpa, const uint8_t *irk) { + struct encryption_block ecb; + + auto irk32 = (const uint32_t *)irk; + auto key32 = (uint32_t *)&ecb.key[0]; + auto pt32 = (uint32_t *)&ecb.plain_text[0]; + + key32[0] = irk32[0]; + key32[1] = irk32[1]; + key32[2] = irk32[2]; + key32[3] = irk32[3]; + + pt32[0] = 0; + pt32[1] = 0; + pt32[2] = 0; + pt32[3] = 0; + + ecb.plain_text[15] = rpa[3]; + ecb.plain_text[14] = rpa[4]; + ecb.plain_text[13] = rpa[5]; + + bt_encrypt_be(ecb.key, ecb.plain_text, ecb.cipher_text); + + if (ecb.cipher_text[15] != rpa[0] || ecb.cipher_text[14] != rpa[1] || ecb.cipher_text[13] != rpa[2]) return false; + + // Serial.printf("RPA resolved %d %02x%02x%02x %02x%02x%02x\r\n", err, rpa[0], rpa[1], rpa[2], ecb.cipher_text[15], ecb.cipher_text[14], ecb.cipher_text[13]); + + return true; +} + +void BleFingerprint::fingerprintAddress() { + auto mac = getMac(); + if (!BleFingerprintCollection::knownMacs.isEmpty() && prefixExists(BleFingerprintCollection::knownMacs, mac)) + setId("known:" + mac, ID_TYPE_KNOWN_MAC); + else { + switch (addressType) { + case BLE_ADDR_PUBLIC: + case BLE_ADDR_PUBLIC_ID: + setId(mac, ID_TYPE_PUBLIC_MAC); + break; + case BLE_ADDR_RANDOM: + case BLE_ADDR_RANDOM_ID: { + const auto *naddress = address.getNative(); + if ((naddress[5] & 0xc0) == 0xc0) + setId(mac, ID_TYPE_RAND_STATIC_MAC); + else { + auto irks = BleFingerprintCollection::irks; + auto it = std::find_if(irks.begin(), irks.end(), [naddress](uint8_t *irk) { return ble_ll_resolv_rpa(naddress, irk); }); + if (it != irks.end()) { + auto irk_hex = hexStr(*it, 16); + setId(std::string("irk:") + irk_hex.c_str(), ID_TYPE_KNOWN_IRK); + break; + } + setId(mac, ID_TYPE_RAND_MAC); + } + break; + } + default: + setId(mac, ID_TYPE_RAND_MAC); + break; + } + } +} + +void BleFingerprint::fingerprintServiceAdvertisements(NimBLEAdvertisedDevice *advertisedDevice, size_t serviceAdvCount, bool haveTxPower, int8_t txPower) { + for (auto i = 0; i < serviceAdvCount; i++) { + auto uuid = advertisedDevice->getServiceUUID(i); +#ifdef VERBOSE + Serial.printf("Verbose | %s | %-58s%ddBm AD: %s\r\n", getMac().c_str(), getId().c_str(), rssi, advertisedDevice->getServiceUUID(i).toString().c_str()); +#endif + if (uuid == tileUUID) { + asRssi = BleFingerprintCollection::rxRefRssi + TILE_TX; + setId("tile:" + getMac(), ID_TYPE_TILE); + return; + } else if (uuid == sonosUUID) { + asRssi = haveTxPower ? BleFingerprintCollection::rxRefRssi + txPower : NO_RSSI; + setId("sonos:" + getMac(), ID_TYPE_SONOS); + return; + } else if (uuid == itagUUID) { + asRssi = BleFingerprintCollection::rxRefRssi + (haveTxPower ? txPower : ITAG_TX); + setId("itag:" + getMac(), ID_TYPE_ITAG); + return; + } else if (uuid == trackrUUID) { + asRssi = haveTxPower ? BleFingerprintCollection::rxRefRssi + txPower : NO_RSSI; + setId("trackr:" + getMac(), ID_TYPE_TRACKR); + return; + } else if (uuid == tractiveUUID) { + asRssi = haveTxPower ? BleFingerprintCollection::rxRefRssi + txPower : NO_RSSI; + setId("tractive:" + getMac(), ID_TYPE_TRACTIVE); + return; + } else if (uuid == vanmoofUUID) { + asRssi = haveTxPower ? BleFingerprintCollection::rxRefRssi + txPower : NO_RSSI; + setId("vanmoof:" + getMac(), ID_TYPE_VANMOOF); + return; + } else if (uuid == (meaterService)) { + asRssi = haveTxPower ? BleFingerprintCollection::rxRefRssi + txPower : NO_RSSI; + setId("meater:" + getMac(), ID_TYPE_MEATER); + return; + } else if (uuid == nutUUID) { + asRssi = BleFingerprintCollection::rxRefRssi + (haveTxPower ? txPower : NUT_TX); + setId("nut:" + getMac(), ID_TYPE_NUT); + return; + } else if (uuid == miFloraUUID) { + asRssi = BleFingerprintCollection::rxRefRssi + (haveTxPower ? txPower : FLORA_TX); + setId("flora:" + getMac(), ID_TYPE_FLORA); + return; + } + } + + std::string fingerprint = "ad:"; + asRssi = haveTxPower ? BleFingerprintCollection::rxRefRssi + txPower : NO_RSSI; + for (int i = 0; i < serviceAdvCount; i++) { + std::string sid = advertisedDevice->getServiceUUID(i).toString(); + fingerprint = fingerprint + sid.c_str(); + } + if (haveTxPower) fingerprint = fingerprint + std::string(-txPower); + setId(fingerprint, ID_TYPE_AD); +} + +void BleFingerprint::fingerprintServiceData(NimBLEAdvertisedDevice *advertisedDevice, size_t serviceDataCount, bool haveTxPower, int8_t txPower) { + asRssi = haveTxPower ? BleFingerprintCollection::rxRefRssi + txPower : NO_RSSI; + std::string fingerprint = ""; + for (int i = 0; i < serviceDataCount; i++) { + BLEUUID uuid = advertisedDevice->getServiceDataUUID(i); + std::string strServiceData = advertisedDevice->getServiceData(i); +#ifdef VERBOSE + Serial.printf("Verbose | %s | %-58s%ddBm SD: %s/%s\r\n", getMac().c_str(), getId().c_str(), rssi, uuid.toString().c_str(), hexStr(strServiceData).c_str()); +#endif + + if (uuid == exposureUUID) { // found COVID-19 exposure tracker + bcnRssi = BleFingerprintCollection::rxRefRssi + EXPOSURE_TX; + setId("exp:" + std::string(strServiceData.length()), ID_TYPE_EXPOSURE); + // disc = hexStr(strServiceData).c_str(); + } else if (uuid == smartTagUUID) { // found Samsung smart tag + asRssi = haveTxPower ? BleFingerprintCollection::rxRefRssi + txPower : NO_RSSI; + setId("smarttag:" + std::string(strServiceData.length()), ID_TYPE_SMARTTAG); + } else if (uuid == miThermUUID) { + asRssi = haveTxPower ? BleFingerprintCollection::rxRefRssi + txPower : NO_RSSI; + if (strServiceData.length() == 15) { // custom format + auto serviceData = strServiceData.c_str(); + temp = float(*(int16_t *)(serviceData + 6)) / 100.0f; + humidity = float(*(uint16_t *)(serviceData + 8)) / 100.0f; + mv = *(uint16_t *)(serviceData + 10); + battery = serviceData[12]; +#ifdef VERBOSE + Serial.printf("Temp: %.1f°, Humidity: %.1f%%, mV: %hu, Battery: %hhu%%, flg: 0x%02hhx, cout: %hhu\r\n", temp, humidity, mv, battery, serviceData[14], serviceData[13]); +#endif + setId("miTherm:" + getMac(), ID_TYPE_MITHERM); + } else if (strServiceData.length() == 13) { // format atc1441 + auto serviceData = strServiceData.c_str(); + int16_t x = (serviceData[6] << 8) | serviceData[7]; + temp = float(x) / 10.0f; + humidity = serviceData[8]; + mv = x = (serviceData[10] << 8) | serviceData[11]; + battery = serviceData[9]; + +#ifdef VERBOSE + Serial.printf("Temp: %.1f°, Humidity: %.1f%%, mV: %hu, Battery: %hhu%%, cout: %hhu\r\n", temp, humidity, mv, battery, serviceData[12]); +#endif + setId("miTherm:" + getMac(), ID_TYPE_MITHERM); + } + } else if (uuid == eddystoneUUID && strServiceData.length() > 0) { + if (strServiceData[0] == EDDYSTONE_URL_FRAME_TYPE && strServiceData.length() <= 18) { + BLEEddystoneURL oBeacon = BLEEddystoneURL(); + oBeacon.setData(strServiceData); + bcnRssi = EDDYSTONE_ADD_1M + oBeacon.getPower(); + } else if (strServiceData[0] == EDDYSTONE_TLM_FRAME_TYPE) { + BLEEddystoneTLM oBeacon = BLEEddystoneTLM(); + oBeacon.setData(strServiceData); + temp = oBeacon.getTemp(); + mv = oBeacon.getVolt(); +#ifdef VERBOSE + Serial.println(oBeacon.toString().c_str()); +#endif + } else if (strServiceData[0] == 0x00) { + auto serviceData = strServiceData.c_str(); + int8_t rss0m = *(int8_t *)(serviceData + 1); + bcnRssi = EDDYSTONE_ADD_1M + rss0m; + setId(Sprintf("eddy:%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x-%02x%02x%02x%02x%02x%02x", + strServiceData[2], strServiceData[3], strServiceData[4], strServiceData[5], strServiceData[6], + strServiceData[6], strServiceData[7], strServiceData[8], strServiceData[9], strServiceData[10], + strServiceData[11], strServiceData[12], strServiceData[13], strServiceData[14], strServiceData[15], + strServiceData[16], strServiceData[17]), + ID_TYPE_EBEACON); + } + } else { + fingerprint = fingerprint + uuid.toString().c_str(); + } + } + if (!fingerprint.isEmpty()) { + if (haveTxPower) fingerprint = fingerprint + std::string(-txPower); + setId("sd:" + fingerprint, ID_TYPE_SD); + } +} + +void BleFingerprint::fingerprintManufactureData(NimBLEAdvertisedDevice *advertisedDevice, bool haveTxPower, int8_t txPower) { + std::string strManufacturerData = advertisedDevice->getManufacturerData(); +#ifdef VERBOSE + Serial.printf("Verbose | %s | %-58s%ddBm MD: %s\r\n", getMac().c_str(), getId().c_str(), rssi, hexStr(strManufacturerData).c_str()); +#endif + if (strManufacturerData.length() >= 2) { + std::string manuf = Sprintf("%02x%02x", strManufacturerData[1], strManufacturerData[0]); + + if (manuf == "004c") // Apple + { + if (strManufacturerData.length() == 25 && strManufacturerData[2] == 0x02 && strManufacturerData[3] == 0x15) { + BLEBeacon oBeacon = BLEBeacon(); + oBeacon.setData(strManufacturerData); + bcnRssi = oBeacon.getSignalPower(); + setId(Sprintf("iBeacon:%s-%u-%u", std::string(oBeacon.getProximityUUID()).c_str(), ENDIAN_CHANGE_U16(oBeacon.getMajor()), ENDIAN_CHANGE_U16(oBeacon.getMinor())), bcnRssi != 3 ? ID_TYPE_IBEACON : ID_TYPE_ECHO_LOST); + } else if (strManufacturerData.length() >= 4 && strManufacturerData[2] == 0x10) { + std::string pid = Sprintf("apple:%02x%02x:%u", strManufacturerData[2], strManufacturerData[3], strManufacturerData.length()); + if (haveTxPower) pid += -txPower; + setId(pid, ID_TYPE_APPLE_NEARBY); + mdRssi = BleFingerprintCollection::rxRefRssi + APPLE_TX; + } else if (strManufacturerData.length() >= 4 && strManufacturerData[2] == 0x12 && strManufacturerData.length() == 29) { + std::string pid = "apple:findmy"; + setId(pid, ID_TYPE_FINDMY); + mdRssi = BleFingerprintCollection::rxRefRssi + APPLE_TX; + } else if (strManufacturerData.length() >= 4) { + std::string pid = Sprintf("apple:%02x%02x:%u", strManufacturerData[2], strManufacturerData[3], strManufacturerData.length()); + if (haveTxPower) pid += -txPower; + setId(pid, ID_TYPE_MISC_APPLE); + mdRssi = BleFingerprintCollection::rxRefRssi + APPLE_TX; + } + } else if (manuf == "05a7") // Sonos + { + mdRssi = haveTxPower ? BleFingerprintCollection::rxRefRssi + txPower : NO_RSSI; + setId("sonos:" + getMac(), ID_TYPE_SONOS); + } else if (manuf == "0087") // Garmin + { + mdRssi = haveTxPower ? BleFingerprintCollection::rxRefRssi + txPower : NO_RSSI; + setId("garmin:" + getMac(), ID_TYPE_GARMIN); + } else if (manuf == "4d4b") // iTrack + { + mdRssi = haveTxPower ? BleFingerprintCollection::rxRefRssi + txPower : NO_RSSI; + setId("iTrack:" + getMac(), ID_TYPE_ITRACK); + } else if (manuf == "0157") // Mi-fit + { + mdRssi = haveTxPower ? BleFingerprintCollection::rxRefRssi + txPower : NO_RSSI; + setId("mifit:" + getMac(), ID_TYPE_MIFIT); + } else if (manuf == "0006" && strManufacturerData.length() == 29) // microsoft + { + mdRssi = haveTxPower ? BleFingerprintCollection::rxRefRssi + txPower : NO_RSSI; + setId(Sprintf("msft:cdp:%02x%02x", strManufacturerData[3], strManufacturerData[5]), ID_TYPE_MSFT); + /*disc = Sprintf("%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x", + strManufacturerData[6], strManufacturerData[7], strManufacturerData[8], strManufacturerData[9], strManufacturerData[10], + strManufacturerData[11], strManufacturerData[12], strManufacturerData[13], strManufacturerData[14], strManufacturerData[15], + strManufacturerData[16], strManufacturerData[17], strManufacturerData[18], strManufacturerData[19], strManufacturerData[20], + strManufacturerData[21], strManufacturerData[22], strManufacturerData[23], strManufacturerData[24], strManufacturerData[25]); + */ + } else if (manuf == "0075") // samsung + { + mdRssi = haveTxPower ? BleFingerprintCollection::rxRefRssi + txPower : NO_RSSI; + setId("samsung:" + getMac(), ID_TYPE_MISC); + } else if (manuf == "beac" && strManufacturerData.length() == 26) { + BLEBeacon oBeacon = BLEBeacon(); + oBeacon.setData(strManufacturerData.substr(0, 25)); + setId(Sprintf("altBeacon:%s-%u-%u", std::string(oBeacon.getProximityUUID()).c_str(), ENDIAN_CHANGE_U16(oBeacon.getMajor()), ENDIAN_CHANGE_U16(oBeacon.getMinor())), ID_TYPE_ABEACON); + bcnRssi = oBeacon.getSignalPower(); + } else if (manuf != "0000") { + mdRssi = haveTxPower ? BleFingerprintCollection::rxRefRssi + txPower : NO_RSSI; + std::string fingerprint = Sprintf("md:%s:%u", manuf.c_str(), strManufacturerData.length()); + if (haveTxPower) fingerprint = fingerprint + std::string(-txPower); + setId(fingerprint, ID_TYPE_MD); + } + } +} + +bool BleFingerprint::seen(BLEAdvertisedDevice *advertisedDevice) { + lastSeenMillis = millis(); + reported = false; + + seenCount++; + + fingerprint(advertisedDevice); + + if (ignore || hidden) return false; + + rssi = advertisedDevice->getRSSI(); + raw = pow(10, float(get1mRssi() - rssi) / (10.0f * BleFingerprintCollection::absorption)); + filteredDistance.addMeasurement(raw); + dist = filteredDistance.getDistance(); + vari = filteredDistance.getVariance(); + + if (!added) { + added = true; + return true; + } + + return false; +} + +bool BleFingerprint::fill(JsonObject *doc) { + (*doc)[F("mac")] = getMac(); + (*doc)[F("id")] = id; + if (!name.isEmpty()) (*doc)[F("name")] = name; + if (idType) (*doc)[F("idType")] = idType; + + (*doc)[F("rssi@1m")] = get1mRssi(); + (*doc)[F("rssi")] = rssi; + + if (isnormal(raw)) (*doc)[F("raw")] = serialized(std::string(raw, 2)); + if (isnormal(dist)) (*doc)[F("distance")] = serialized(std::string(dist, 2)); + if (isnormal(vari)) (*doc)[F("var")] = serialized(std::string(vari, 2)); + if (close) (*doc)[F("close")] = true; + + (*doc)[F("int")] = (millis() - firstSeenMillis) / seenCount; + + if (mv) (*doc)[F("mV")] = mv; + if (battery != 0xFF) (*doc)[F("batt")] = battery; + if (temp) (*doc)[F("temp")] = serialized(std::string(temp, 1)); + if (humidity) (*doc)[F("rh")] = serialized(std::string(humidity, 1)); + return true; +} + +bool BleFingerprint::report(JsonObject *doc) { + if (ignore || idType <= ID_TYPE_RAND_MAC || hidden) return false; + if (reported) return false; + + auto maxDistance = BleFingerprintCollection::maxDistance; + if (maxDistance > 0 && dist > maxDistance) + return false; + + auto now = millis(); + if ((abs(dist - lastReported) < BleFingerprintCollection::skipDistance) && (lastReportedMillis > 0) && (now - lastReportedMillis < BleFingerprintCollection::skipMs)) + return false; + + if (fill(doc)) { + lastReportedMillis = now; + lastReported = dist; + reported = true; + return true; + } + + return false; +} + +bool BleFingerprint::query() { + if (!allowQuery || isQuerying) return false; + if (rssi < -90) return false; // Too far away + + auto now = millis(); + if (now - lastSeenMillis > 5) return false; // Haven't seen lately + if (now - lastQryMillis < qryDelayMillis) return false; // Too soon + + isQuerying = true; + lastQryMillis = now; + + bool success = false; + + Serial.printf("%u Query | %s | %-58s%ddBm %lums\r\n", xPortGetCoreID(), getMac().c_str(), id.c_str(), rssi, now - lastSeenMillis); + + NimBLEClient *pClient = NimBLEDevice::getClientListSize() ? NimBLEDevice::getClientByPeerAddress(address) : nullptr; + if (!pClient) pClient = NimBLEDevice::getDisconnectedClient(); + if (!pClient) pClient = NimBLEDevice::createClient(); + pClient->setClientCallbacks(&clientCB, false); + pClient->setConnectionParams(12, 12, 0, 48); + pClient->setConnectTimeout(5); + NimBLEDevice::getScan()->stop(); + if (pClient->connect(address)) { + if (allowQuery) { + if (id.startsWith("flora:")) + success = MiFloraHandler::requestData(pClient, this); + else + success = NameModelHandler::requestData(pClient, this); + } + } + + NimBLEDevice::deleteClient(pClient); + + if (success) { + qryAttempts = 0; + qryDelayMillis = BleFingerprintCollection::requeryMs; + } else { + qryAttempts++; + qryDelayMillis = min(int(pow(10, qryAttempts)), 60000); + Serial.printf("%u QryErr | %s | %-58s%ddBm Try %d, retry after %dms\r\n", xPortGetCoreID(), getMac().c_str(), id.c_str(), rssi, qryAttempts, qryDelayMillis); + } + isQuerying = false; + return true; +} + +bool BleFingerprint::shouldCount() { + if (!close && rssi > CLOSE_RSSI + BleFingerprintCollection::rxAdjRssi) { + BleFingerprintCollection::Close(this, true); + close = true; + } else if (close && rssi < LEFT_RSSI + BleFingerprintCollection::rxAdjRssi) { + BleFingerprintCollection::Close(this, false); + close = false; + } + + bool prevCounting = counting; + if (ignore || !countable) + counting = false; + else if (getMsSinceLastSeen() > BleFingerprintCollection::countMs) + counting = false; + else if (counting && dist > BleFingerprintCollection::countExit) + counting = false; + else if (!counting && dist <= BleFingerprintCollection::countEnter) + counting = true; + + if (prevCounting != counting) { + BleFingerprintCollection::Count(this, counting); + } + + return counting; +} + +void BleFingerprint::expire() { + lastSeenMillis = 0; +} diff --git a/components/esp32_presense/BleFingerprint.h b/components/esp32_presense/BleFingerprint.h new file mode 100644 index 0000000..c566f7f --- /dev/null +++ b/components/esp32_presense/BleFingerprint.h @@ -0,0 +1,160 @@ +#ifndef _BLEFINGERPRINT_ +#define _BLEFINGERPRINT_ +#include +#include +#include "NimBLEAdvertisedDevice.h" +#include "NimBLEBeacon.h" +#include "NimBLEEddystoneTLM.h" +#include "NimBLEEddystoneURL.h" + +#include + +#include "QueryReport.h" +#include "rssi.h" +#include "string_utils.h" +#include "FilteredDistance.h" + +#include "esphome/core/log.h" +#include "esphome/core/helpers.h" +#include "esphome/core/hal.h" +#include "esphome/core/time.h" + +#define log_e ESP_LOGE +#define millis esphome::millis + +#define NO_RSSI int8_t(-128) + + + +#define ID_TYPE_TX_POW short(1) + +#define NO_ID_TYPE short(0) + +#define ID_TYPE_ECHO_LOST short(-10) +#define ID_TYPE_MISC_APPLE short(-5) + +#define ID_TYPE_RAND_MAC short(1) +#define ID_TYPE_RAND_STATIC_MAC short(5) +#define ID_TYPE_AD short(10) +#define ID_TYPE_SD short(15) +#define ID_TYPE_MD short(20) +#define ID_TYPE_MISC short(30) +#define ID_TYPE_FINDMY short(32) +#define ID_TYPE_NAME short(35) +#define ID_TYPE_MSFT short(40) +#define ID_TYPE_UNIQUE short(50) +#define ID_TYPE_PUBLIC_MAC short(55) +#define ID_TYPE_SONOS short(105) +#define ID_TYPE_GARMIN short(107) +#define ID_TYPE_MITHERM short(110) +#define ID_TYPE_MIFIT short(115) +#define ID_TYPE_EXPOSURE short(120) +#define ID_TYPE_SMARTTAG short(121) +#define ID_TYPE_ITAG short(125) +#define ID_TYPE_ITRACK short(127) +#define ID_TYPE_NUT short(128) +#define ID_TYPE_FLORA short(129) +#define ID_TYPE_TRACKR short(130) +#define ID_TYPE_TILE short(135) +#define ID_TYPE_MEATER short(140) +#define ID_TYPE_TRACTIVE short(142) +#define ID_TYPE_VANMOOF short(145) +#define ID_TYPE_APPLE_NEARBY short(150) +#define ID_TYPE_QUERY_MODEL short(155) +#define ID_TYPE_QUERY_NAME short(160) +#define ID_TYPE_RM_ASST short(165) +#define ID_TYPE_EBEACON short(170) +#define ID_TYPE_ABEACON short(175) +#define ID_TYPE_IBEACON short(180) +#define ID_TYPE_KNOWN_IRK short(200) +#define ID_TYPE_KNOWN_MAC short(210) +#define ID_TYPE_ALIAS short(250) + +class BleFingerprint { + public: + BleFingerprint(NimBLEAdvertisedDevice *advertisedDevice, float fcmin, float beta, float dcutoff); + + bool seen(BLEAdvertisedDevice *advertisedDevice); + + bool fill(JsonObject *doc); + + bool report(JsonObject *doc); + + bool query(); + + const std::string getId() const { return id; } + + const std::string getName() const { return name; } + + void setName(const std::string &name) { this->name = name; } + + bool setId(const std::string &newId, short int newIdType, const std::string &newName = ""); + + void setInitial(const BleFingerprint &other); + + const std::string getMac() const; + + const short getIdType() const { return idType; } + + const float getDistance() const { return dist; } + + const int getRssi() const { return rssi; } + const int getRawRssi() const { return rssi; } + + const int get1mRssi() const; + void set1mRssi(int8_t rssi) { calRssi = rssi; } + + const NimBLEAddress getAddress() const { return address; } + + const unsigned long getMsSinceLastSeen() const { return lastSeenMillis ? millis() - lastSeenMillis : 4294967295; }; + + const unsigned long getMsSinceFirstSeen() const { return millis() - firstSeenMillis; }; + + const bool getVisible() const { return !ignore && !hidden; } + + const bool getAdded() const { return added; }; + + const bool getIgnore() const { return ignore; }; + + const bool getAllowQuery() const { return allowQuery; }; + + const bool hasReport() { return queryReport != nullptr; }; + const QueryReport getReport() { return *queryReport; }; + void setReport(const QueryReport &report) { queryReport = std::unique_ptr(new QueryReport{report}); }; + void clearReport() { queryReport.reset(); }; + + unsigned int getSeenCount() { + auto sc = seenCount - lastSeenCount; + lastSeenCount = seenCount; + return sc; + } + + bool shouldCount(); + void fingerprintAddress(); + void expire(); + + private: + + bool added = false, close = false, reported = false, ignore = false, allowQuery = false, isQuerying = false, hidden = false, connectable = false, countable = false, counting = false; + NimBLEAddress address; + std::string id, name; + short int idType = NO_ID_TYPE; + int rssi = NO_RSSI; + int8_t calRssi = NO_RSSI, bcnRssi = NO_RSSI, mdRssi = NO_RSSI, asRssi = NO_RSSI; + unsigned int qryAttempts = 0, qryDelayMillis = 0; + float raw = 0, dist = 0, vari = 0, lastReported = 0, temp = 0, humidity = 0; + unsigned long firstSeenMillis, lastSeenMillis = 0, lastReportedMillis = 0, lastQryMillis = 0; + unsigned long seenCount = 1, lastSeenCount = 0; + uint16_t mv = 0; + uint8_t battery = 0xFF, addressType = 0xFF; + FilteredDistance filteredDistance; + std::unique_ptr queryReport = nullptr; + + static bool shouldHide(const std::string &s); + void fingerprint(NimBLEAdvertisedDevice *advertisedDevice); + void fingerprintServiceAdvertisements(NimBLEAdvertisedDevice *advertisedDevice, size_t serviceAdvCount, bool haveTxPower, int8_t txPower); + void fingerprintServiceData(NimBLEAdvertisedDevice *advertisedDevice, size_t serviceDataCount, bool haveTxPower, int8_t txPower); + void fingerprintManufactureData(NimBLEAdvertisedDevice *advertisedDevice, bool haveTxPower, int8_t txPower); +}; + +#endif diff --git a/components/esp32_presense/BleFingerprintCollection.cpp b/components/esp32_presense/BleFingerprintCollection.cpp new file mode 100644 index 0000000..55e929b --- /dev/null +++ b/components/esp32_presense/BleFingerprintCollection.cpp @@ -0,0 +1,260 @@ +#include "BleFingerprintCollection.h" + +#include "esphome/core/application.h" +#include "esphome/core/defines.h" +#include "esphome/core/util.h" +#include "esphome/core/log.h" +#include "esphome/components/network/util.h" + +#include "esp_system.h" + +#include + +namespace BleFingerprintCollection { +// Public (externed) +std::string include{}, exclude{}, query{}, knownMacs{}, knownIrks{}, countIds{}; +float skipDistance = 0.0f, maxDistance = 0.0f, absorption = 3.5f, countEnter = 2, countExit = 4; +int8_t rxRefRssi = -65, rxAdjRssi = 0, txRefRssi = -59; +int forgetMs = 0, skipMs = 0, countMs = 10000, requeryMs = 300000; +std::vector deviceConfigs; +std::vector irks; +std::vector fingerprints; +TCallbackBool onSeen = nullptr; +TCallbackFingerprint onAdd = nullptr; +TCallbackFingerprint onDel = nullptr; +TCallbackFingerprint onClose = nullptr; +TCallbackFingerprint onLeft = nullptr; +TCallbackFingerprint onCountAdd = nullptr; +TCallbackFingerprint onCountDel = nullptr; + +// Private +const TickType_t MAX_WAIT = portTICK_PERIOD_MS * 100; + +unsigned long lastCleanup = 0; +SemaphoreHandle_t fingerprintMutex; +SemaphoreHandle_t deviceConfigMutex; + +void Setup() { + fingerprintMutex = xSemaphoreCreateMutex(); + deviceConfigMutex = xSemaphoreCreateMutex(); +} + +void Count(BleFingerprint *f, bool counting) { + if (counting) { + if (onCountAdd) onCountAdd(f); + } else { + if (onCountDel) onCountDel(f); + } +} + +void Close(BleFingerprint *f, bool close) { + if (close) { + if (onClose) onClose(f); + } else { + if (onLeft) onLeft(f); + } +} + +void Seen(BLEAdvertisedDevice *advertisedDevice) { + BLEAdvertisedDevice copy = *advertisedDevice; + + if (onSeen) onSeen(true); + BleFingerprint *f = GetFingerprint(©); + if (f->seen(©) && onAdd) + onAdd(f); + if (onSeen) onSeen(false); +} + +bool addOrReplace(DeviceConfig config) { + if (xSemaphoreTake(deviceConfigMutex, MAX_WAIT) != pdTRUE) + log_e("Couldn't take deviceConfigMutex in addOrReplace!"); + + for (auto &it : deviceConfigs) { + if (it.id == config.id) { + it = config; + xSemaphoreGive(deviceConfigMutex); + return false; + } + } + deviceConfigs.push_back(config); + xSemaphoreGive(deviceConfigMutex); + return true; +} + +bool Config(std::string &id, std::string &json) { + DynamicJsonDocument doc(1024); + deserializeJson(doc, json); + + DeviceConfig config = {}; + config.id = id; + if (doc.containsKey("id")) { + auto alias = doc["id"].as(); + if (alias != id) config.alias = alias; + } + if (doc.containsKey("rssi@1m")) + config.calRssi = doc["rssi@1m"].as(); + if (doc.containsKey("name")) + config.name = doc["name"].as(); + auto isNew = addOrReplace(config); + + if (isNew) { + auto p = id.find("irk:"); + if (p == 0) { + auto irk_hex = id.substr(4); + auto *irk = new uint8_t[16]; + if (!hextostr(irk_hex, irk, 16)) + return false; + irks.push_back(irk); + } + } + + for (auto &it : fingerprints) { + auto it_id = it->getId(); + if (it_id == id || it_id == config.alias) { + it->setName(config.name); + it->setId(config.alias.length() > 0 ? config.alias : config.id, ID_TYPE_ALIAS, config.name); + if (config.calRssi != NO_RSSI) + it->set1mRssi(config.calRssi); + } else + it->fingerprintAddress(); + } + + return true; +} + +void ConnectToWifi() { + std::istringstream iss(knownIrks.c_str()); + std::string irk_hex; + while (iss >> irk_hex) { + auto *irk = new uint8_t[16]; + if (!hextostr(irk_hex.c_str(), irk, 16)) + continue; + irks.push_back(irk); + } +} + +bool Command(std::string &command, std::string &pay) { + if (command == "skip_ms") { + BleFingerprintCollection::skipMs = std::stoi(pay); + spurt("/skip_ms", pay); + } else if (command == "skip_distance") { + BleFingerprintCollection::skipDistance = std::stof(pay); + spurt("/skip_dist", pay); + } else if (command == "max_distance") { + maxDistance = std::stof(pay); + spurt("/max_dist", pay); + } else if (command == "absorption") { + absorption = std::stof(pay); + spurt("/absorption", pay); + } else if (command == "rx_adj_rssi") { + rxAdjRssi = (int8_t)std::stoi(pay); + spurt("/rx_adj_rssi", pay); + } else if (command == "ref_rssi") { + rxRefRssi = (int8_t)std::stoi(pay); + spurt("/ref_rssi", pay); + } else if (command == "tx_ref_rssi") { + txRefRssi = (int8_t)std::stoi(pay); + spurt("/tx_ref_rssi", pay); + } else if (command == "query") { + query = pay; + spurt("/query", pay); + } else if (command == "include") { + include = pay; + spurt("/include", pay); + } else if (command == "exclude") { + exclude = pay; + spurt("/exclude", pay); + } else if (command == "known_macs") { + knownMacs = pay; + spurt("/known_macs", pay); + } else if (command == "known_irks") { + knownIrks = pay; + spurt("/known_irks", pay); + } else if (command == "count_ids") { + countIds = pay; + spurt("/count_ids", pay); + } else + return false; + return true; +} + +void CleanupOldFingerprints() { + auto now = millis(); + if (now - lastCleanup < 5000) return; + lastCleanup = now; + auto it = fingerprints.begin(); + bool any = false; + while (it != fingerprints.end()) { + auto age = (*it)->getMsSinceLastSeen(); + if (age > forgetMs) { + if (onDel) onDel((*it)); + delete *it; + it = fingerprints.erase(it); + } else { + any = true; + ++it; + } + } + if (!any) { + auto uptime = (unsigned long)(esp_timer_get_time() / 1000000ULL); + if (uptime > ALLOW_BLE_CONTROLLER_RESTART_AFTER_SECS) { + ESP_LOGE("Bluetooth controller seems stuck, restarting"); + esp_restart(); + } + } +} + +BleFingerprint *getFingerprintInternal(BLEAdvertisedDevice *advertisedDevice) { + auto mac = advertisedDevice->getAddress(); + + auto it = std::find_if(fingerprints.rbegin(), fingerprints.rend(), [mac](BleFingerprint *f) { return f->getAddress() == mac; }); + if (it != fingerprints.rend()) + return *it; + + auto created = new BleFingerprint(advertisedDevice, ONE_EURO_FCMIN, ONE_EURO_BETA, ONE_EURO_DCUTOFF); + auto it2 = std::find_if(fingerprints.begin(), fingerprints.end(), [created](BleFingerprint *f) { return f->getId() == created->getId(); }); + if (it2 != fingerprints.end()) { + auto found = *it2; + // Serial.printf("Detected mac switch for fingerprint id %s\r\n", found->getId().c_str()); + created->setInitial(*found); + if (found->getIdType() > ID_TYPE_UNIQUE) + found->expire(); + } + + fingerprints.push_back(created); + return created; +} + +BleFingerprint *GetFingerprint(BLEAdvertisedDevice *advertisedDevice) { + if (xSemaphoreTake(fingerprintMutex, MAX_WAIT) != pdTRUE) + log_e("Couldn't take semaphore!"); + auto f = getFingerprintInternal(advertisedDevice); + xSemaphoreGive(fingerprintMutex); + return f; +} + +const std::vector GetCopy() { + if (xSemaphoreTake(fingerprintMutex, MAX_WAIT) != pdTRUE) + log_e("Couldn't take fingerprintMutex!"); + CleanupOldFingerprints(); + std::vector copy(fingerprints); + xSemaphoreGive(fingerprintMutex); + return std::move(copy); +} + +bool FindDeviceConfig(const std::string &id, DeviceConfig &config) { + if (xSemaphoreTake(deviceConfigMutex, MAX_WAIT) == pdTRUE) { + auto it = std::find_if(deviceConfigs.begin(), deviceConfigs.end(), [id](DeviceConfig dc) { return dc.id == id; }); + if (it != deviceConfigs.end()) { + config = *it; + xSemaphoreGive(deviceConfigMutex); + return true; + } + xSemaphoreGive(deviceConfigMutex); + return false; + } + log_e("Couldn't take deviceConfigMutex!"); + return false; +} + +} // namespace BleFingerprintCollection diff --git a/components/esp32_presense/BleFingerprintCollection.h b/components/esp32_presense/BleFingerprintCollection.h new file mode 100644 index 0000000..ea83bdb --- /dev/null +++ b/components/esp32_presense/BleFingerprintCollection.h @@ -0,0 +1,54 @@ +#pragma once +#include + +#include "BleFingerprint.h" + +#define ONE_EURO_FCMIN 1e-1f +#define ONE_EURO_BETA 1e-3f +#define ONE_EURO_DCUTOFF 5e-3f + +#ifndef ALLOW_BLE_CONTROLLER_RESTART_AFTER_SECS +#define ALLOW_BLE_CONTROLLER_RESTART_AFTER_SECS 1800 +#endif + +struct DeviceConfig { + std::string id; + std::string alias; + std::string name; + int8_t calRssi = NO_RSSI; +}; + +namespace BleFingerprintCollection { + +typedef std::function TCallbackBool; +typedef std::function TCallbackFingerprint; + +void Setup(); +void ConnectToWifi(); +bool Command(std::string &command, std::string &pay); +bool Config(std::string &id, std::string &json); + +void Close(BleFingerprint *f, bool close); +void Count(BleFingerprint *f, bool counting); +void Seen(BLEAdvertisedDevice *advertisedDevice); +BleFingerprint *GetFingerprint(BLEAdvertisedDevice *advertisedDevice); +void CleanupOldFingerprints(); +const std::vector GetCopy(); +bool FindDeviceConfig(const std::string &id, DeviceConfig &config); + +extern TCallbackBool onSeen; +extern TCallbackFingerprint onAdd; +extern TCallbackFingerprint onDel; +extern TCallbackFingerprint onClose; +extern TCallbackFingerprint onLeft; +extern TCallbackFingerprint onCountAdd; +extern TCallbackFingerprint onCountDel; + +extern std::string include, exclude, query, knownMacs, knownIrks, countIds; +extern float skipDistance, maxDistance, absorption, countEnter, countExit; +extern int8_t rxRefRssi, rxAdjRssi, txRefRssi; +extern int forgetMs, skipMs, countMs, requeryMs; +extern std::vector deviceConfigs; +extern std::vector irks; +extern std::vector fingerprints; +} // namespace BleFingerprintCollection diff --git a/components/esp32_presense/FilteredDistance.cpp b/components/esp32_presense/FilteredDistance.cpp new file mode 100644 index 0000000..26b32e6 --- /dev/null +++ b/components/esp32_presense/FilteredDistance.cpp @@ -0,0 +1,76 @@ +#include "FilteredDistance.h" + +#include +#include +#include +#include "esphome/core/hal.h" + +FilteredDistance::FilteredDistance(float minCutoff, float beta, float dcutoff) + : minCutoff(minCutoff), beta(beta), dcutoff(dcutoff), x(0), dx(0), lastDist(0), lastTime(0), readIndex(0), total(0), totalSquared(0) { +} + +void FilteredDistance::initSpike(float dist) { + for (size_t i = 0; i < NUM_READINGS; i++) { + readings[i] = dist; + } + total = dist * NUM_READINGS; + totalSquared = dist * dist * NUM_READINGS; // Initialize sum of squared distances +} + +float FilteredDistance::removeSpike(float dist) { + total -= readings[readIndex]; // Subtract the last reading + totalSquared -= readings[readIndex] * readings[readIndex]; // Subtract the square of the last reading + + readings[readIndex] = dist; // Read the sensor + total += readings[readIndex]; // Add the reading to the total + totalSquared += readings[readIndex] * readings[readIndex]; // Add the square of the reading + + readIndex = (readIndex + 1) % NUM_READINGS; // Advance to the next position in the array + + auto average = total / static_cast(NUM_READINGS); // Calculate the average + + if (std::fabs(dist - average) > SPIKE_THRESHOLD) + return average; // Spike detected, use the average as the filtered value + + return dist; // No spike, return the new value +} + +void FilteredDistance::addMeasurement(float dist) { + const bool initialized = lastTime != 0; + const unsigned long now = esphome::micros(); + const unsigned long elapsed = now - lastTime; + lastTime = now; + + if (!initialized) { + x = dist; // Set initial filter state to the first reading + dx = 0; // Initial derivative is unknown, so we set it to zero + lastDist = dist; + initSpike(dist); + } else { + float dT = std::max(elapsed * 0.000001f, 0.05f); // Convert microseconds to seconds, enforce a minimum dT + const float alpha = getAlpha(minCutoff, dT); + const float dAlpha = getAlpha(dcutoff, dT); + + dist = removeSpike(dist); + x += alpha * (dist - x); + dx = dAlpha * ((dist - lastDist) / dT); + lastDist = x + beta * dx; + } +} + +const float FilteredDistance::getDistance() const { + return lastDist; +} + +float FilteredDistance::getAlpha(float cutoff, float dT) { + float tau = 1.0f / (2 * M_PI * cutoff); + return 1.0f / (1.0f + tau / dT); +} + +const float FilteredDistance::getVariance() const { + auto mean = total / static_cast(NUM_READINGS); + auto meanOfSquares = totalSquared / static_cast(NUM_READINGS); + auto variance = meanOfSquares - (mean * mean); // Variance formula: E(X^2) - (E(X))^2 + if (variance < 0.0f) return 0.0f; + return variance; +} diff --git a/components/esp32_presense/FilteredDistance.h b/components/esp32_presense/FilteredDistance.h new file mode 100644 index 0000000..e08210f --- /dev/null +++ b/components/esp32_presense/FilteredDistance.h @@ -0,0 +1,37 @@ +#ifndef FILTEREDDISTANCE_H +#define FILTEREDDISTANCE_H + + + +#define SPIKE_THRESHOLD 1.0f // Threshold for spike detection +#define NUM_READINGS 12 // Number of readings to keep track of + +class FilteredDistance { + private: + float minCutoff; + float beta; + float dcutoff; + float x, dx; + float lastDist; + unsigned long lastTime; + + float getAlpha(float cutoff, float dT); + + float readings[NUM_READINGS]; // Array to store readings + int readIndex; // Current position in the array + float total; // Total of the readings + float totalSquared; // Total of the squared readings + + void initSpike(float dist); + float removeSpike(float dist); + public: + FilteredDistance(float minCutoff = 1.0f, float beta = 0.0f, float dcutoff = 1.0f); + void addMeasurement(float dist); + const float getMedianDistance() const; + const float getDistance() const; + const float getVariance() const; + + bool hasValue() const { return lastTime != 0; } +}; + +#endif // FILTEREDDISTANCE_H diff --git a/components/esp32_presense/MiFloraHandler.cpp b/components/esp32_presense/MiFloraHandler.cpp new file mode 100644 index 0000000..78bd7a0 --- /dev/null +++ b/components/esp32_presense/MiFloraHandler.cpp @@ -0,0 +1,137 @@ +#include "MiFloraHandler.h" + +namespace MiFloraHandler { + +std::vector addresses; +bool readSensorData(BLERemoteService* floraService, DynamicJsonDocument* doc) { + BLERemoteCharacteristic* floraCharacteristic = nullptr; + + // get the main device data characteristic + floraCharacteristic = floraService->getCharacteristic(uuid_sensor_data); + + if (floraCharacteristic == nullptr) { + ESP_LOGD("-- Can't read characteristics"); + return false; + } + + // read characteristic value + NimBLEAttValue value; + + value = floraCharacteristic->readValue(); + + if (value.size() == 0) { + ESP_LOGD("Reading Value failed"); + return false; + } + + const char* val = value.c_str(); + + float temperature = (val[0] + val[1] * 256) / ((float)10.0); + uint8_t moisture = val[7]; + uint32_t brightness = val[3] + val[4] * 256; + float conductivity = val[8] + val[9] * 256; + + (*doc)[F("temperature")] = temperature; + (*doc)[F("moisture")] = moisture; + (*doc)[F("light")] = brightness; + (*doc)[F("conductivity")] = conductivity; + + floraService->deleteCharacteristics(); + + return true; +} + +bool readBatteryData(BLERemoteService* floraService, DynamicJsonDocument* doc) { + BLERemoteCharacteristic* floraCharacteristic = nullptr; + + floraCharacteristic = floraService->getCharacteristic(uuid_version_battery); + + if (floraCharacteristic == nullptr) { + ESP_LOGD("-- Can't read characteristics"); + return false; + } + NimBLEAttValue val; + + val = floraCharacteristic->readValue(); + + if (val.size() == 0) { + ESP_LOGD("Reading Value failed"); + return false; + } + + int8_t battery = val.c_str()[0]; + (*doc)[F("battery")] = battery; + + floraService->deleteCharacteristics(); + return true; +} + +bool forceFloraServiceDataMode(BLERemoteService* floraService) { // Setting the mi flora to data reading mode + BLERemoteCharacteristic* floraCharacteristic; + + // get device mode characteristic, needs to be changed to read data + // Serial.println("- Force device in data mode"); + floraCharacteristic = nullptr; + floraCharacteristic = floraService->getCharacteristic(uuid_write_mode); + + if (floraCharacteristic == nullptr) { + // Serial.println("-- Failed, skipping device"); + return false; + } + + uint8_t buf[2] = {0xA0, 0x1F}; + floraCharacteristic->writeValue(buf, 2, true); + + delay(500); + floraService->deleteCharacteristics(); + + return true; +} + +void fillDeviceData(DynamicJsonDocument* doc, BleFingerprint* f) { + (*doc)[F("id")] = f->getId(); + (*doc)[F("mac")] = f->getMac(); + (*doc)[F("rssi")] = f->getRssi(); +} + +bool getFloraData(DynamicJsonDocument* doc, BLERemoteService* floraService, BleFingerprint* f) { + // Force miFlora to data mode + + fillDeviceData(doc, f); + + if (!MiFloraHandler::readBatteryData(floraService, doc)) + ESP_LOGD("Failed reading battery data"); + + if (MiFloraHandler::forceFloraServiceDataMode(floraService)) { + } else { + ESP_LOGD("Failed to force data reading mode"); + } + + if (!MiFloraHandler::readSensorData(floraService, doc)) + ESP_LOGD("Failed reading sensor data"); + + return true; +} + +bool requestData(NimBLEClient* pClient, BleFingerprint* fingerprint) // Getting mi flora data +{ + DynamicJsonDocument document(256); + + NimBLERemoteService* floraService = pClient->getService(serviceUUID); + + if (floraService == nullptr) { + ESP_LOGD("Getting MiFlora service failed"); + return false; + } + + // Retriving the actual data + if (!getFloraData(&document, floraService, fingerprint)) // Getting flora data + return false; + std::string buf = std::string(); + serializeJson(document, buf); + // Sending buffer over mqtt + fingerprint->setReport(QueryReport{"miflora", buf}); + return true; +} + +} // namespace MiFloraHandler diff --git a/components/esp32_presense/MiFloraHandler.h b/components/esp32_presense/MiFloraHandler.h new file mode 100644 index 0000000..44196ab --- /dev/null +++ b/components/esp32_presense/MiFloraHandler.h @@ -0,0 +1,15 @@ +#pragma once +#include +#include +#include "NimBLEClient.h" +#include "NimBLEDevice.h" + +namespace MiFloraHandler +{ + static BLEUUID serviceUUID(0x00001204, 0x0000, 0x1000, 0x800000805f9b34fb); + static BLEUUID uuid_version_battery(0x00001a02, 0x0000, 0x1000, 0x800000805f9b34fb); + static BLEUUID uuid_sensor_data(0x00001a01, 0x0000, 0x1000, 0x800000805f9b34fb); + static BLEUUID uuid_write_mode(0x00001a00, 0x0000, 0x1000, 0x800000805f9b34fb); + + bool requestData(NimBLEClient* pClient, BleFingerprint* fingerprint); +} // namespace MiFloraHandler diff --git a/components/esp32_presense/NameModelHandler.cpp b/components/esp32_presense/NameModelHandler.cpp new file mode 100644 index 0000000..63453e9 --- /dev/null +++ b/components/esp32_presense/NameModelHandler.cpp @@ -0,0 +1,24 @@ +#include "NameModelHandler.h" +#include "util.h" + +namespace NameModelHandler { +bool requestData(NimBLEClient* pClient, BleFingerprint* f) { + std::string sMdl = pClient->getValue(deviceInformationService, modelChar); + std::string sName = pClient->getValue(genericAccessService, nameChar); + if (!sName.empty() && sMdl.find(sName) == std::string::npos && sName != "Apple Watch") { + if (f->setId(std::string("name:") + kebabify(sName).c_str(), ID_TYPE_QUERY_NAME, std::string(sName.c_str()))) { + ESP_LOGD("\u001b[38;5;104m%u Name | %s | %-58s%ddBm %s\u001b[0m\r\n", xPortGetCoreID(), f->getMac().c_str(), f->getId().c_str(), f->getRssi(), sName.c_str()); + } + return true; + } + + if (!sMdl.empty()) { + if (f->setId(std::string("apple:") + kebabify(sMdl).c_str(), ID_TYPE_QUERY_MODEL, std::string(sMdl.c_str()))) { + ESP_LOGD("\u001b[38;5;136m%u Model | %s | %-58s%ddBm %s\u001b[0m\r\n", xPortGetCoreID(), f->getMac().c_str(), f->getId().c_str(), f->getRssi(), sMdl.c_str()); + } + return true; + } + + return false; +} +} // namespace NameModelHandler diff --git a/components/esp32_presense/NameModelHandler.h b/components/esp32_presense/NameModelHandler.h new file mode 100644 index 0000000..68c3e6c --- /dev/null +++ b/components/esp32_presense/NameModelHandler.h @@ -0,0 +1,11 @@ +#pragma once +#include +#include +#include +#include +#include "BleFingerprint.h" + +namespace NameModelHandler { + +bool requestData(NimBLEClient* pClient, BleFingerprint* fingerprint); +} diff --git a/components/esp32_presense/QueryReport.h b/components/esp32_presense/QueryReport.h new file mode 100644 index 0000000..f075a6e --- /dev/null +++ b/components/esp32_presense/QueryReport.h @@ -0,0 +1,17 @@ +#pragma once +#include + +class QueryReport { +public: + QueryReport(const std::string& id, const std::string& payload) : id(id), payload(payload) {} + + std::string getId() const { return id; } + std::string getPayload() const { return payload; } + + void setId(const std::string& id) { this->id = id; } + void setPayload(const std::string& payload) { this->payload = payload; } + +private: + std::string id; + std::string payload; +}; diff --git a/components/esp32_presense/__init__.py b/components/esp32_presense/__init__.py new file mode 100644 index 0000000..c018593 --- /dev/null +++ b/components/esp32_presense/__init__.py @@ -0,0 +1,52 @@ +import esphome.config_validation as cv +import esphome.codegen as cg +from esphome.const import ( + CONF_ID, +) +from esphome.components import mqtt, nimble_tracker +from esphome.components.esp32 import add_idf_sdkconfig_option + +DEPENDENCIES = ["mqtt"] + +# CONF_NIMBLE_ID = "esp32_nimble_mqtt_room" + +CONF_ROOM_KEY = 'room' +CONF_BASE_TOPIC_KEY = 'base_topic' +CONF_MAC_KEY = 'mac_addr' +CONF_MAX_DISTANCE = 'max_distance' + +esp32_nimble_tracker_ns = cg.esphome_ns.namespace("esp32_presense") +ESP32Presense = esp32_nimble_tracker_ns.class_( + "ESP32Presense", cg.Component +) + +CONFIG_SCHEMA = cv.Schema({ + cv.GenerateID(): cv.declare_id(ESP32Presense), + cv.Required(CONF_ROOM_KEY): cv.string, + cv.Required(CONF_MAC_KEY): cv.All(cv.ensure_list(cv.string)), + cv.Optional(CONF_MAX_DISTANCE, default=16.0): cv.float_, + cv.Optional(CONF_BASE_TOPIC_KEY, default="esphome_presense"): cv.string, +}).extend(cv.COMPONENT_SCHEMA) + +async def to_code(config): + var = cg.new_Pvariable(config[CONF_ID]) + await cg.register_component(var, config) + + cg.add(var.set_room(config[CONF_ROOM_KEY])) + cg.add(var.set_base_topic(config[CONF_BASE_TOPIC_KEY])) + cg.add(var.set_addresses(config[CONF_MAC_KEY])) + cg.add(var.set_max_distance(config[CONF_MAX_DISTANCE])) + + add_idf_sdkconfig_option("CONFIG_BT_ENABLED", True) + add_idf_sdkconfig_option("CONFIG_BT_BLUEDROID_ENABLED", False) + add_idf_sdkconfig_option("CONFIG_BT_NIMBLE_ENABLED", True) + add_idf_sdkconfig_option("CONFIG_MBEDTLS_HARDWARE_AES", False) + + cg.add_library( + name="esp-nimble-cpp", + repository="https://github.com/h2zero/esp-nimble-cpp", + version="1.4.1" + ) + + # await nimble_tracker.device_listener_to_code(var, config) + # await nimble_tracker.register_ble_device(var, config) \ No newline at end of file diff --git a/components/esp32_presense/esp32_presense.cpp b/components/esp32_presense/esp32_presense.cpp new file mode 100644 index 0000000..5bfd776 --- /dev/null +++ b/components/esp32_presense/esp32_presense.cpp @@ -0,0 +1,81 @@ +#include "esp32_presense.h" + +namespace esphome +{ + namespace esp32_nimble_mqtt_room + { + unsigned int totalSeen = 0; + unsigned int totalFpSeen = 0; + unsigned int totalFpQueried = 0; + unsigned int totalFpReported = 0; + + + + void ESP32Presense::reportLoop() + { + auto copy = BleFingerprintCollection::GetCopy(); + unsigned int count = 0; + for (auto &i : copy) + if (i->shouldCount()) + count++; + sendTelemetry(totalSeen, totalFpSeen, totalFpQueried, totalFpReported, count); + + auto reported = 0; + for (auto &f : copy) { + auto seen = f->getSeenCount(); + if (seen) { + totalSeen += seen; + totalFpSeen++; + } + + /* + if (f->hasReport()) { + if (reportBuffer(f)) + f->clearReport(); + } + + if (reportDevice(f)) { + totalFpReported++; + reported++; + } + */ + } + } + + bool ESP32Presense::sendTelemetry( + unsigned int totalSeen, + unsigned int totalFpSeen, + unsigned int totalFpQueried, + unsigned int totalFpReported, + unsigned int count + ) { + this->publish(this->room_ + "/status", "online"); + this->publish(this->room_ + "/max_distance", BleFingerprintCollection::maxDistance); + this->publish(this->room_ + "/absorption", BleFingerprintCollection::absorption); + this->publish(this->room_ + "/tx_ref_rssi", BleFingerprintCollection::txRefRssi); + this->publish(this->room_ + "/rx_adj_rssi", BleFingerprintCollection::rxAdjRssi); + this->publish(this->room_ + "/query", BleFingerprintCollection::query); + this->publish(this->room_ + "/include", BleFingerprintCollection::include); + this->publish(this->room_ + "/exclude", BleFingerprintCollection::exclude); + this->publish(this->room_ + "/known_macs", BleFingerprintCollection::knownMacs); + this->publish(this->room_ + "/known_irks", BleFingerprintCollection::knownIrks); + this->publish(this->room_ + "/count_ids", BleFingerprintCollection::countIds); + + return true; + } + /* + void ESP32NimbleMQTTRoom::on_result(nimble_distance_custom::NimbleDistanceCustomResult& result) + { + auto address = result.address.toString(); + + this->publish_json( + this->base_topic_ + "/devices/" + address + "/" + this->room_, + [=](ArduinoJson::JsonObject root) -> void { + root["id"] = address; + root["distance"] = result.distance; + } + ); + }; + */ + } // namespace esp32_nimble_tracker +} // namespace esphome \ No newline at end of file diff --git a/components/esp32_presense/esp32_presense.h b/components/esp32_presense/esp32_presense.h new file mode 100644 index 0000000..8b97be3 --- /dev/null +++ b/components/esp32_presense/esp32_presense.h @@ -0,0 +1,37 @@ +#pragma once + +#include "esphome/core/component.h" +#include "esphome/components/mqtt/custom_mqtt_device.h" +#include "BleFingerprintCollection.h" +// #include "esphome/components/nimble_distance_custom/nimble_distance_custom.h" + +namespace esphome +{ + namespace esp32_nimble_mqtt_room + { + class ESP32Presense : + public mqtt::CustomMQTTDevice + // public nimble_distance_custom::NimbleDistanceCustomComponent + { + protected: + std::string room_; + + BleFingerprint *ble_fingerprint = NULL; + + bool sendTelemetry( + unsigned int totalSeen, + unsigned int totalFpSeen, + unsigned int totalFpQueried, + unsigned int totalFpReported, + unsigned int count + ); + public: + void setup() {}; + void loop() {}; + void reportLoop(); + // void on_result(nimble_distance_custom::NimbleDistanceCustomResult&) override; + void set_room(std::string room) { room_ = room; } + //void set_base_topic(std::string base_topic) { base_topic_ = base_topic; } + }; + } // namespace esp32_nimble_tracker +} // namespace esphome \ No newline at end of file diff --git a/components/esp32_presense/rssi.h b/components/esp32_presense/rssi.h new file mode 100644 index 0000000..e42d07f --- /dev/null +++ b/components/esp32_presense/rssi.h @@ -0,0 +1,19 @@ +#ifndef _RSSI_ +#define _RSSI_ + +#define CLOSE_RSSI int8_t(-40) +#define LEFT_RSSI int8_t(-50) + +#define DEFAULT_TX int8_t(-6) + +#define APPLE_TX int8_t(0) +#define RM_ASST_TX int8_t(0) +#define TILE_TX int8_t(-4) +#define EXPOSURE_TX int8_t(-12) +#define ITAG_TX int8_t(-10) + +#define NUT_TX int8_t(-12) +#define FLORA_TX int8_t(-10) + +#define EDDYSTONE_ADD_1M int8_t(-41) +#endif diff --git a/components/esp32_presense/string_utils.cpp b/components/esp32_presense/string_utils.cpp new file mode 100644 index 0000000..7b86c7e --- /dev/null +++ b/components/esp32_presense/string_utils.cpp @@ -0,0 +1,134 @@ +#include +#include "string_utils.h" +// #include + +static constexpr char hexmap[] = {'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'a', 'b', 'c', 'd', 'e', 'f'}; + +std::string ltrim(const std::string &s, char toTrim) +{ + size_t start = s.find_first_not_of(toTrim); + return (start == std::string::npos) ? "" : s.substr(start); +} + +std::string rtrim(const std::string &s, char toTrim) +{ + size_t end = s.find_last_not_of(toTrim); + return (end == std::string::npos) ? "" : s.substr(0, end + 1); +} + +std::string lowertrim(std::string str, char toTrim) +{ + std::transform(str.begin(), str.end(), str.begin(), [](unsigned char c) + { return std::tolower(c); }); + return rtrim(ltrim(str, toTrim), toTrim); +} + +std::regex whitespace_re("[\\s\\W-]+"); +std::string slugify(const std::string &text) +{ + return lowertrim(std::regex_replace(text, whitespace_re, "_"), '_'); +} + +std::string kebabify(const std::string &text) +{ + return lowertrim(std::regex_replace(text, whitespace_re, "-"), '-'); +} + +std::string hexStr(const uint8_t *data, int len) { + std::string s(len * 2, ' '); + for (int i = 0; i < len; ++i) { + s[2 * i] = hexmap[(data[i] & 0xF0) >> 4]; + s[2 * i + 1] = hexmap[data[i] & 0x0F]; + } + return s; +} + +std::string hexStr(const char *data, unsigned int len) +{ + std::string s(len * 2, ' '); + for (int i = 0; i < len; ++i) + { + s[2 * i] = hexmap[(data[i] & 0xF0) >> 4]; + s[2 * i + 1] = hexmap[data[i] & 0x0F]; + } + return s; +} + +std::string hexStr(const std::string &s) +{ + return hexStr(s.c_str(), s.length()); +} + +std::string hexStr(const uint8_t *&s, unsigned int len) +{ + return hexStr(reinterpret_cast(s), len); +} + +std::string hexStrRev(const char *data, unsigned int len) +{ + std::string s(len * 2, ' '); + for (int i = 0; i < len; ++i) + { + s[len - (2 * i + 1)] = hexmap[(data[i] & 0xF0) >> 4]; + s[len - (2 * i + 2)] = hexmap[data[i] & 0x0F]; + } + return s; +} + +std::string hexStrRev(const uint8_t *&s, unsigned int len) +{ + return hexStrRev(reinterpret_cast(s), len); +} + +std::string hexStrRev(const std::string &s) +{ + return hexStrRev(s.c_str(), s.length()); +} + +uint8_t hextob(char ch) +{ + if (ch >= '0' && ch <= '9') return ch - '0'; + if (ch >= 'A' && ch <= 'F') return ch - 'A' + 10; + if (ch >= 'a' && ch <= 'f') return ch - 'a' + 10; + return 0; +} + +bool hextostr(const std::string &hexStr, uint8_t* output, size_t len) +{ + if (hexStr.length() & 1) return false; + if (hexStr.length() < len*2) return false; + int k = 0; + for (size_t i = 0; i < len*2; i+=2) + output[k++] = (hextob(hexStr[i]) << 4) | hextob(hexStr[i+1]); + return true; +} + +bool prefixExists(const std::string &prefixes, const std::string &s) +{ + unsigned int start = 0; + unsigned int space; + + while ((space = prefixes.find(" ", start)) != -1) + { + if (space > start) + { + auto sub = prefixes.substr(start, space); + if (s.find(sub) == 0) return true; + } + start = space + 1; + } + auto sub = prefixes.substr(start); + return !(sub.length() == 0) && s.find(sub) == 0; +} + +bool spurt(const std::string &fn, const std::string &content) +{ + /* + File f = SPIFFS.open(fn, "w"); + if (!f) return false; + auto w = f.print(content); + f.close(); + return w == content.length(); + */ + return true; +} diff --git a/components/esp32_presense/string_utils.h b/components/esp32_presense/string_utils.h new file mode 100644 index 0000000..e9a0cec --- /dev/null +++ b/components/esp32_presense/string_utils.h @@ -0,0 +1,20 @@ +#pragma once +#include +#include + +#define CHIPID (uint32_t)(ESP.getEfuseMac() >> 24) +#define ESPMAC (Sprintf("%06x", CHIPID)) +#define Sprintf(f, ...) ({ char* s; asprintf(&s, f, __VA_ARGS__); const std::string r = s; free(s); r; }) +#define Stdprintf(f, ...) ({ char* s; asprintf(&s, f, __VA_ARGS__); const std::string r = s; free(s); r; }) + +std::string slugify(const std::string& text); +std::string kebabify(const std::string& text); +std::string hexStr(const uint8_t *data, int len); +std::string hexStr(const char *data, int len); +std::string hexStr(const std::string& s); +std::string hexStrRev(const uint8_t *data, int len); +std::string hexStrRev(const char *data, int len); +std::string hexStrRev(const std::string &s); +bool hextostr(const std::string &hexStr, uint8_t* output, size_t len); +bool prefixExists(const std::string &prefixes, const std::string &s); +bool spurt(const std::string &fn, const std::string &content); diff --git a/components/esp32_presense/util.h b/components/esp32_presense/util.h new file mode 100644 index 0000000..53f57c3 --- /dev/null +++ b/components/esp32_presense/util.h @@ -0,0 +1,37 @@ +#include +#include +#include + +#define ENDIAN_CHANGE_U16(x) ((((x)&0xFF00) >> 8) + (((x)&0xFF) << 8)) + +const BLEUUID eddystoneUUID((uint16_t)0xFEAA); +const BLEUUID tileUUID((uint16_t)0xFEED); +const BLEUUID exposureUUID((uint16_t)0xFD6F); +const BLEUUID smartTagUUID((uint16_t)0xFD5A); +const BLEUUID sonosUUID((uint16_t)0xFE07); +const BLEUUID itagUUID((uint16_t)0xffe0); +const BLEUUID miThermUUID(uint16_t(0x181A)); +const BLEUUID trackrUUID((uint16_t)0x0F3E); +const BLEUUID vanmoofUUID(0x6acc5540, 0xe631, 0x4069, 0x944db8ca7598ad50); +const BLEUUID tractiveUUID(0x20130001, 0x0719, 0x4b6e, 0xbe5d158ab92fa5a4); +const BLEUUID espresenseUUID(0xe5ca1ade, 0xf007, 0xba11, 0x0000000000000000); + +const BLEUUID nutUUID((uint16_t)0x1803); +const BLEUUID miFloraUUID((uint16_t)0xfe95); + +const BLEUUID fitbitUUID(0xadabfb00, 0x6e7d, 0x4601, 0xbda2bffaa68956ba); + +const BLEUUID roomAssistantService(0x5403c8a7, 0x5c96, 0x47e9, 0x9ab859e373d875a7); +const BLEUUID rootAssistantCharacteristic(0x21c46f33, 0xe813, 0x4407, 0x86012ad281030052); + +const BLEUUID meaterService(0xa75cc7fc, 0xc956, 0x488f, 0xac2a2dbc08b63a04); +const BLEUUID meaterCharacteristic(0x7edda774, 0x045e, 0x4bbf, 0x909b45d1991a2876); + +const BLEUUID genericAccessService(uint16_t(0x1800)); +const BLEUUID nameChar(uint16_t(0x2A00)); + +const BLEUUID deviceInformationService(uint16_t(0x180A)); +const BLEUUID modelChar(uint16_t(0x2A24)); +const BLEUUID fwRevChar(uint16_t(0x2A26)); +const BLEUUID hwRevChar(uint16_t(0x2A27)); +const BLEUUID manufChar(uint16_t(0x2A29));