wip
This commit is contained in:
parent
f0a839667d
commit
e464f33da3
553
components/esp32_presense/BleFingerprint.cpp
Normal file
553
components/esp32_presense/BleFingerprint.cpp
Normal file
@ -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;
|
||||
}
|
||||
160
components/esp32_presense/BleFingerprint.h
Normal file
160
components/esp32_presense/BleFingerprint.h
Normal file
@ -0,0 +1,160 @@
|
||||
#ifndef _BLEFINGERPRINT_
|
||||
#define _BLEFINGERPRINT_
|
||||
#include <ArduinoJson.h>
|
||||
#include <NimBLEDevice.h>
|
||||
#include "NimBLEAdvertisedDevice.h"
|
||||
#include "NimBLEBeacon.h"
|
||||
#include "NimBLEEddystoneTLM.h"
|
||||
#include "NimBLEEddystoneURL.h"
|
||||
|
||||
#include <memory>
|
||||
|
||||
#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<QueryReport>(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> 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
|
||||
260
components/esp32_presense/BleFingerprintCollection.cpp
Normal file
260
components/esp32_presense/BleFingerprintCollection.cpp
Normal file
@ -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 <sstream>
|
||||
|
||||
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<DeviceConfig> deviceConfigs;
|
||||
std::vector<uint8_t *> irks;
|
||||
std::vector<BleFingerprint *> 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<std::string>();
|
||||
if (alias != id) config.alias = alias;
|
||||
}
|
||||
if (doc.containsKey("rssi@1m"))
|
||||
config.calRssi = doc["rssi@1m"].as<int8_t>();
|
||||
if (doc.containsKey("name"))
|
||||
config.name = doc["name"].as<std::string>();
|
||||
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<BleFingerprint *> GetCopy() {
|
||||
if (xSemaphoreTake(fingerprintMutex, MAX_WAIT) != pdTRUE)
|
||||
log_e("Couldn't take fingerprintMutex!");
|
||||
CleanupOldFingerprints();
|
||||
std::vector<BleFingerprint *> 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
|
||||
54
components/esp32_presense/BleFingerprintCollection.h
Normal file
54
components/esp32_presense/BleFingerprintCollection.h
Normal file
@ -0,0 +1,54 @@
|
||||
#pragma once
|
||||
#include <ArduinoJson.h>
|
||||
|
||||
#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<void(bool)> TCallbackBool;
|
||||
typedef std::function<void(BleFingerprint *)> 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<BleFingerprint *> 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<DeviceConfig> deviceConfigs;
|
||||
extern std::vector<uint8_t *> irks;
|
||||
extern std::vector<BleFingerprint *> fingerprints;
|
||||
} // namespace BleFingerprintCollection
|
||||
76
components/esp32_presense/FilteredDistance.cpp
Normal file
76
components/esp32_presense/FilteredDistance.cpp
Normal file
@ -0,0 +1,76 @@
|
||||
#include "FilteredDistance.h"
|
||||
|
||||
#include <cmath>
|
||||
#include <numeric>
|
||||
#include <vector>
|
||||
#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<float>(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<float>(NUM_READINGS);
|
||||
auto meanOfSquares = totalSquared / static_cast<float>(NUM_READINGS);
|
||||
auto variance = meanOfSquares - (mean * mean); // Variance formula: E(X^2) - (E(X))^2
|
||||
if (variance < 0.0f) return 0.0f;
|
||||
return variance;
|
||||
}
|
||||
37
components/esp32_presense/FilteredDistance.h
Normal file
37
components/esp32_presense/FilteredDistance.h
Normal file
@ -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
|
||||
137
components/esp32_presense/MiFloraHandler.cpp
Normal file
137
components/esp32_presense/MiFloraHandler.cpp
Normal file
@ -0,0 +1,137 @@
|
||||
#include "MiFloraHandler.h"
|
||||
|
||||
namespace MiFloraHandler {
|
||||
|
||||
std::vector<std::string> 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
|
||||
15
components/esp32_presense/MiFloraHandler.h
Normal file
15
components/esp32_presense/MiFloraHandler.h
Normal file
@ -0,0 +1,15 @@
|
||||
#pragma once
|
||||
#include <ArduinoJson.h>
|
||||
#include <BleFingerprint.h>
|
||||
#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
|
||||
24
components/esp32_presense/NameModelHandler.cpp
Normal file
24
components/esp32_presense/NameModelHandler.cpp
Normal file
@ -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
|
||||
11
components/esp32_presense/NameModelHandler.h
Normal file
11
components/esp32_presense/NameModelHandler.h
Normal file
@ -0,0 +1,11 @@
|
||||
#pragma once
|
||||
#include <NimBLEClient.h>
|
||||
#include <NimBLEDevice.h>
|
||||
#include <ArduinoJson.h>
|
||||
#include <sstream>
|
||||
#include "BleFingerprint.h"
|
||||
|
||||
namespace NameModelHandler {
|
||||
|
||||
bool requestData(NimBLEClient* pClient, BleFingerprint* fingerprint);
|
||||
}
|
||||
17
components/esp32_presense/QueryReport.h
Normal file
17
components/esp32_presense/QueryReport.h
Normal file
@ -0,0 +1,17 @@
|
||||
#pragma once
|
||||
#include <NimBLEAddress.h>
|
||||
|
||||
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;
|
||||
};
|
||||
52
components/esp32_presense/__init__.py
Normal file
52
components/esp32_presense/__init__.py
Normal file
@ -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)
|
||||
81
components/esp32_presense/esp32_presense.cpp
Normal file
81
components/esp32_presense/esp32_presense.cpp
Normal file
@ -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
|
||||
37
components/esp32_presense/esp32_presense.h
Normal file
37
components/esp32_presense/esp32_presense.h
Normal file
@ -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
|
||||
19
components/esp32_presense/rssi.h
Normal file
19
components/esp32_presense/rssi.h
Normal file
@ -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
|
||||
134
components/esp32_presense/string_utils.cpp
Normal file
134
components/esp32_presense/string_utils.cpp
Normal file
@ -0,0 +1,134 @@
|
||||
#include <regex>
|
||||
#include "string_utils.h"
|
||||
// #include <SPIFFS.h>
|
||||
|
||||
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<const char *>(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<const char *>(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;
|
||||
}
|
||||
20
components/esp32_presense/string_utils.h
Normal file
20
components/esp32_presense/string_utils.h
Normal file
@ -0,0 +1,20 @@
|
||||
#pragma once
|
||||
#include <string>
|
||||
#include <cstring>
|
||||
|
||||
#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);
|
||||
37
components/esp32_presense/util.h
Normal file
37
components/esp32_presense/util.h
Normal file
@ -0,0 +1,37 @@
|
||||
#include <NimBLEBeacon.h>
|
||||
#include <NimBLEDevice.h>
|
||||
#include <string>
|
||||
|
||||
#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));
|
||||
Loading…
Reference in New Issue
Block a user