ESPresense/lib/BleFingerprint/BleFingerprint.cpp

366 lines
15 KiB
C++

#include "BleFingerprint.h"
#include "BleFingerprintCollection.h"
#include "rssi.h"
#include "strings.h"
#include "util.h"
#define Sprintf(f, ...) ( \
{ \
char *s; \
asprintf(&s, f, __VA_ARGS__); \
String r = s; \
free(s); \
r; \
})
#define SMacf(f) ( \
{ \
auto nativeAddress = f.getNative(); \
Sprintf("%02x%02x%02x%02x%02x%02x", nativeAddress[5], nativeAddress[4], nativeAddress[3], nativeAddress[2], nativeAddress[1], nativeAddress[0]); \
})
String BleFingerprint::getMac() { return SMacf(address); }
int BleFingerprint::get1mRssi()
{
if (calRssi != NO_RSSI) return calRssi;
if (mdRssi != NO_RSSI) return mdRssi;
if (asRssi != NO_RSSI) return asRssi;
return _parent->getRefRssi() + DEFAULT_TX;
}
BleFingerprint::BleFingerprint(BleFingerprintCollection *parent, BLEAdvertisedDevice *advertisedDevice, float fcmin, float beta, float dcutoff) : oneEuro{OneEuroFilter<float, long long>(1, fcmin, beta, dcutoff)}
{
_parent = parent;
firstSeenMillis = millis();
address = advertisedDevice->getAddress();
macPublic = advertisedDevice->getAddressType() == BLE_ADDR_PUBLIC;
newest = recent = oldest = rssi = advertisedDevice->getRSSI();
}
void BleFingerprint::fingerprint(BLEAdvertisedDevice *advertisedDevice)
{
if (advertisedDevice->haveName())
name = String(advertisedDevice->getName().c_str());
if (advertisedDevice->haveServiceUUID())
{
#ifdef VERBOSE
for (int i = 0; i < advertisedDevice->getServiceUUIDCount(); i++)
Serial.printf("Verbose | %-58sSD: %s\n", getId().c_str(), advertisedDevice->getServiceUUID(i).toString().c_str());
#endif
if (advertisedDevice->isAdvertisingService(tileUUID))
{
calRssi = _parent->getRefRssi() + TILE_TX;
setId("tile:" + getMac(), ID_TYPE_TILE);
}
else if (advertisedDevice->isAdvertisingService(exposureUUID))
{ // found covid exposure tracker
std::string strServiceData = advertisedDevice->getServiceData(exposureUUID);
calRssi = _parent->getRefRssi() + EXPOSURE_TX;
setId("exp:" + String(strServiceData.length()), ID_TYPE_EXPOSURE);
name = hexStr(strServiceData).c_str();
}
else if (advertisedDevice->isAdvertisingService(sonosUUID))
{
asRssi = advertisedDevice->haveTXPower() ? _parent->getRefRssi() + advertisedDevice->getTXPower() : NO_RSSI;
setId("sonos:" + getMac(), ID_TYPE_SONOS);
}
else if (advertisedDevice->isAdvertisingService(itagUUID))
{
asRssi = _parent->getRefRssi() + (advertisedDevice->haveTXPower() ? advertisedDevice->getTXPower() : ITAG_TX);
setId("itag:" + getMac(), ID_TYPE_ITAG);
}
else if (advertisedDevice->isAdvertisingService(roomAssistantService))
{
asRssi = advertisedDevice->haveTXPower() ? advertisedDevice->getTXPower() - 65 : NO_RSSI;
shouldQuery = true;
rmAsst = true;
}
else if (advertisedDevice->isAdvertisingService(eddystoneUUID))
{
std::string strServiceData = advertisedDevice->getServiceData(eddystoneUUID);
if (strServiceData[0] == EDDYSTONE_URL_FRAME_TYPE && strServiceData.length() <= 18)
{
BLEEddystoneURL oBeacon = BLEEddystoneURL();
oBeacon.setData(strServiceData);
calRssi = EDDYSTONE_ADD_1M + oBeacon.getPower();
}
else if (strServiceData[0] == EDDYSTONE_TLM_FRAME_TYPE)
{
BLEEddystoneTLM oBeacon = BLEEddystoneTLM();
oBeacon.setData(strServiceData);
temp = oBeacon.getTemp();
volts = oBeacon.getVolt();
#ifdef VERBOSE
Serial.println(oBeacon.toString().c_str());
#endif
}
}
else
{
asRssi = advertisedDevice->haveTXPower() ? _parent->getRefRssi() + advertisedDevice->getTXPower() : NO_RSSI;
String fingerprint = "sid:";
for (int i = 0; i < advertisedDevice->getServiceUUIDCount(); i++)
{
std::string sid = advertisedDevice->getServiceUUID(i).toString();
fingerprint = fingerprint + String(sid.c_str());
}
if (advertisedDevice->haveTXPower())
fingerprint = fingerprint + String(-advertisedDevice->getTXPower());
setId(fingerprint, ID_TYPE_SID);
}
}
if (advertisedDevice->haveManufacturerData())
{
std::string strManufacturerData = advertisedDevice->getManufacturerData();
#ifdef VERBOSE
Serial.printf("Verbose | %-58sMD: %s\n", getId().c_str(), hexStr(strManufacturerData).c_str());
#endif
if (strManufacturerData.length() >= 2)
{
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);
setId(Sprintf("iBeacon:%s-%d-%d", std::string(oBeacon.getProximityUUID()).c_str(), ENDIAN_CHANGE_U16(oBeacon.getMajor()), ENDIAN_CHANGE_U16(oBeacon.getMinor())), ID_TYPE_IBEACON);
calRssi = oBeacon.getSignalPower();
}
else if (strManufacturerData.length() >= 4 && strManufacturerData[2] == 0x10)
{
shouldQuery = true;
ignore = false;
{
String pid;
if (advertisedDevice->haveTXPower())
pid = Sprintf("apple:%02x%02x:%d%d", strManufacturerData[2], strManufacturerData[3], strManufacturerData.length(), -advertisedDevice->getTXPower());
else
pid = Sprintf("apple:%02x%02x:%d", strManufacturerData[2], strManufacturerData[3], strManufacturerData.length());
setId(pid, ID_TYPE_APPLE_NEARBY);
}
disc = hexStr(strManufacturerData.substr(4)).c_str();
mdRssi = _parent->getRefRssi() + APPLE_TX;
}
else
{
if (advertisedDevice->haveTXPower())
setId(Sprintf("apple:%02x%02x:%d%d", strManufacturerData[2], strManufacturerData[3], strManufacturerData.length(), -advertisedDevice->getTXPower()), ID_TYPE_MISC_APPLE + ID_TYPE_TX_POW);
else
setId(Sprintf("apple:%02x%02x:%d", strManufacturerData[2], strManufacturerData[3], strManufacturerData.length()), ID_TYPE_MISC_APPLE);
mdRssi = _parent->getRefRssi() + APPLE_TX;
ignore = true;
}
}
else if (manuf == "05a7") //Sonos
{
mdRssi = advertisedDevice->haveTXPower() ? _parent->getRefRssi() + advertisedDevice->getTXPower() : NO_RSSI;
setId("sonos:" + getMac(), ID_TYPE_SONOS);
}
else if (manuf == "0157") //Mi-fit
{
mdRssi = advertisedDevice->haveTXPower() ? _parent->getRefRssi() + advertisedDevice->getTXPower() : NO_RSSI;
setId("mifit:" + getMac(), ID_TYPE_MIFIT);
}
else if (manuf == "0006" && strManufacturerData.length() == 29) //microsoft
{
mdRssi = advertisedDevice->haveTXPower() ? _parent->getRefRssi() + advertisedDevice->getTXPower() : NO_RSSI;
setId("md:microsoft:29", 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%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], strManufacturerData[26], strManufacturerData[27], strManufacturerData[28]);
ignore = true;
}
else if (manuf == "0075") //samsung
{
mdRssi = advertisedDevice->haveTXPower() ? _parent->getRefRssi() + advertisedDevice->getTXPower() : NO_RSSI;
setId("samsung:" + getMac(), ID_TYPE_MISC);
}
else
{
mdRssi = advertisedDevice->haveTXPower() ? _parent->getRefRssi() + advertisedDevice->getTXPower() : NO_RSSI;
String fingerprint = Sprintf("md:%s:%d", manuf.c_str(), strManufacturerData.length());
if (advertisedDevice->haveTXPower())
fingerprint = fingerprint + String(-advertisedDevice->getTXPower());
setId(fingerprint, ID_TYPE_MD);
}
}
}
}
bool BleFingerprint::filter()
{
Reading<float, long long> inter1, inter2;
inter1.timestamp = esp_timer_get_time();
inter1.value = raw;
return oneEuro.push(&inter1, &inter2) && diffFilter.push(&inter2, &output);
}
bool BleFingerprint::seen(BLEAdvertisedDevice *advertisedDevice)
{
lastSeenMillis = millis();
seenCount++;
if (ignore) return false;
oldest = recent;
recent = newest;
newest = advertisedDevice->getRSSI();
rssi = median_of_3(oldest, recent, newest);
fingerprint(advertisedDevice);
if (ignore) return false;
float ratio = (get1mRssi() - rssi) / 35.0f;
raw = pow(10, ratio);
bool hadValue = hasValue;
if (filter())
{
hasValue = true;
reported = false;
if (!close && newest > CLOSE_RSSI)
{
Display.close(this);
close = true;
}
else if (close && newest < LEFT_RSSI)
{
Display.left(this);
close = false;
}
if (!hadValue) return true;
}
return false;
}
void BleFingerprint::setInitial(int initalRssi, float initalDistance)
{
newest = recent = oldest = rssi = initalRssi;
raw = initalDistance;
hasValue = filter() || filter();
}
bool BleFingerprint::report(JsonDocument *doc)
{
if (ignore || (idType == 0 && !macPublic))
return false;
if (reported || !hasValue)
return false;
auto maxDistance = _parent->getMaxDistance();
if (maxDistance > 0 && output.value.position > maxDistance)
return false;
auto now = millis();
if ((abs(output.value.position - lastReported) < _parent->getSkipDistance()) && (lastReportedMillis > 0) && (now - lastReportedMillis < _parent->getSkipMs()))
return false;
//Serial.printf("%f > %f || %d > %d\n", abs(output.value.position - lastReported), _parent->getSkipDistance(), now - lastReportedMillis, _parent->getSkipMs());
lastReportedMillis = now;
lastReported = output.value.position;
reported = true;
(*doc)[F("id")] = getId();
if (!name.isEmpty()) (*doc)[F("name")] = name;
if (!disc.isEmpty()) (*doc)[F("disc")] = disc;
(*doc)[F("rssi@1m")] = get1mRssi();
(*doc)[F("rssi")] = rssi;
(*doc)[F("mac")] = SMacf(address);
(*doc)[F("raw")] = round(raw * 100.0f) / 100.0f;
(*doc)[F("distance")] = round(output.value.position * 100.0f) / 100.0f;
(*doc)[F("speed")] = round(output.value.speed * 1e7f) / 10.0f;
if (volts) (*doc)[F("volts")] = volts;
if (temp) (*doc)[F("temp")] = temp;
return true;
}
bool BleFingerprint::query()
{
if (!shouldQuery || didQuery) return false;
auto maxDistance = _parent->getMaxDistance();
if (!maxDistance) maxDistance = 16;
if (!hasValue || output.value.position > maxDistance)
return false;
auto now = millis();
if (now - lastQryMillis < 500) return false;
didQuery = true;
lastQryMillis = now;
NimBLEClient *pClient = NimBLEDevice::getClientListSize() ? NimBLEDevice::getClientByPeerAddress(address) : nullptr;
if (!pClient) pClient = NimBLEDevice::getDisconnectedClient();
if (!pClient) pClient = NimBLEDevice::createClient();
pClient->setConnectTimeout(5);
if (pClient->connect(address))
{
if (name.isEmpty())
{
auto sName = pClient->getValue(genericAccessService, nameChar);
if (!sName.empty())
{
Serial.printf("%d Name | MAC: %s, ID: %-60s %s\n", xPortGetCoreID(), getMac().c_str(), getId().c_str(), sName.c_str());
if (name.length() == 0) name = sName.c_str();
}
}
auto sRmAst = pClient->getValue(roomAssistantService, rootAssistantCharacteristic);
if (!sRmAst.empty())
{
Serial.printf("%d RmAst | MAC: %s, ID: %-60s %s\n", xPortGetCoreID(), getMac().c_str(), getId().c_str(), sRmAst.c_str());
setId(String("roomAssistant:") + kebabify(sRmAst).c_str(), ID_TYPE_RM_ASST);
}
else
{
auto sMdl = pClient->getValue(deviceInformationService, modelChar);
if (!sMdl.empty())
{
Serial.printf("%d Model | MAC: %s, ID: %-60s %s\n", xPortGetCoreID(), getMac().c_str(), getId().c_str(), sMdl.c_str());
setId(String("apple:") + kebabify(sMdl).c_str(), ID_TYPE_APPLE_MODEL);
if (name.isEmpty()) name = sMdl.c_str();
}
else
{
if (name.length() > 0) setId(String("name:") + kebabify(name), ID_TYPE_NAME);
}
}
// auto sFwRevChar = pClient->getValue(deviceInformationService, fwRevChar);
// Serial.printf("%d FwRev | MAC: %s, ID: %-50s%s\n", xPortGetCoreID(), getMac().c_str(), getId().c_str(), sFwRevChar.c_str());
// auto sHwRevChar = pClient->getValue(deviceInformationService, hwRevChar);
// Serial.printf("%d HwRev | MAC: %s, ID: %-50s%s\n", xPortGetCoreID(), getMac().c_str(), getId().c_str(), sHwRevChar.c_str());
pClient->disconnect();
}
else
{
qryAttempts++;
if (qryAttempts < 10) didQuery = false;
}
return true;
}