diff --git a/lib/BleFingerprint/BleFingerprint.cpp b/lib/BleFingerprint/BleFingerprint.cpp index 6396c21..1c2018b 100644 --- a/lib/BleFingerprint/BleFingerprint.cpp +++ b/lib/BleFingerprint/BleFingerprint.cpp @@ -408,11 +408,6 @@ void BleFingerprint::fingerprintManufactureData(NimBLEAdvertisedDevice *advertis void BleChannelObservation::observe(unsigned long timestamp, int rssi1m, int rssi) { this->rssi = rssi; raw = pow(10, float(rssi1m - rssi) / (10.0f * BleFingerprintCollection::absorption)); - filter.addMeasurement(raw); - dist = filter.getDistance(); - vari = filter.getVariance(); - ci = 1.959 * std::sqrt(vari) / std::sqrt(12); - mean = filter.getMeanDistance(); lastSeenMillis = timestamp; } @@ -428,6 +423,18 @@ bool BleFingerprint::seen(BLEAdvertisedDevice *advertisedDevice, uint8_t channel lastChannel = channel; channels[ble_channel_to_index(channel)].observe(millis(), get1mRssi(), advertisedDevice->getRSSI()); + int maxRssiRecent = NO_RSSI, maxRssiOverall = NO_RSSI; + for (const auto& channel : channels) { + if (channel.rssi > maxRssiOverall) + maxRssiOverall = channel.rssi; + if (channel.rssi > maxRssiRecent && channel.lastSeenMillis > millis() - 5000) + maxRssiRecent = channel.rssi; + } + + int rssi = (maxRssiRecent != NO_RSSI) ? maxRssiRecent : maxRssiOverall; + auto raw = pow(10, float(get1mRssi() - rssi) / (10.0f * BleFingerprintCollection::absorption)); + filter.addMeasurement(raw); + if (!added) { added = true; return true; @@ -443,71 +450,30 @@ bool BleFingerprint::fill(JsonObject *doc) { if (idType) (*doc)[F("idType")] = idType; (*doc)[F("rssi@1m")] = get1mRssi(); - (*doc)[F("rssi")] = getMaxObservedRssi(); + (*doc)[F("rssi")] = channels[ble_channel_to_index(lastChannel)].rssi; + (*doc)[F("raw")] = serialized(String(channels[ble_channel_to_index(lastChannel)].raw, 2)); + switch (lastChannel) { case 37: (*doc)[F("rssi37")] = channels[0].rssi; (*doc)[F("raw37")] = serialized(String(channels[0].raw, 2)); - (*doc)[F("distance37")] = serialized(String(channels[0].dist, 2)); - (*doc)[F("var37")] = serialized(String(channels[0].vari, 2)); - (*doc)[F("mean37")] = serialized(String(channels[0].mean, 2)); - (*doc)[F("ci37")] = serialized(String(channels[0].ci, 2)); break; case 38: (*doc)[F("rssi38")] = channels[1].rssi; (*doc)[F("raw38")] = serialized(String(channels[1].raw, 2)); - (*doc)[F("distance38")] = serialized(String(channels[1].dist, 2)); - (*doc)[F("var38")] = serialized(String(channels[1].vari, 2)); - (*doc)[F("mean38")] = serialized(String(channels[1].mean, 2)); - (*doc)[F("ci39")] = serialized(String(channels[1].ci, 2)); break; case 39: (*doc)[F("rssi39")] = channels[2].rssi; (*doc)[F("raw39")] = serialized(String(channels[2].raw, 2)); - (*doc)[F("distance39")] = serialized(String(channels[2].dist, 2)); - (*doc)[F("var39")] = serialized(String(channels[2].vari, 2)); - (*doc)[F("mean39")] = serialized(String(channels[2].mean, 2)); - (*doc)[F("ci39")] = serialized(String(channels[2].ci, 2)); break; } (*doc)[F("channel")] = lastChannel; - float weightedDistances = 0, weightedMeans = 0, weightedVariances = 0, weightedCIs = 0; - float sumWeights = 0; - int channelCount = 0; - for (const auto& channel : channels) { - if (channel.lastSeenMillis < millis() - 5000) - continue; - float weight = 1 / std::max(channel.vari, 0.05f); - weightedDistances += channel.dist * weight; - weightedMeans += channel.mean * weight; - weightedVariances += channel.vari * weight; - weightedCIs += channel.ci * weight; - sumWeights += weight; - channelCount++; - } - // FIXME: weight channels by timestamp of last packet, so we can ignore stale values? - if (!channelCount) { - weightedDistances = channels[0].dist; - weightedMeans = channels[0].mean; - weightedVariances = channels[0].vari; - sumWeights = 1; - channelCount = 1; - } - float fusedDistance = weightedDistances / sumWeights; - (*doc)[F("distance")] = serialized(String(fusedDistance, 2)); + (*doc)[F("distance")] = serialized(String(filter.getDistance(), 2)); + (*doc)[F("mean")] = serialized(String(filter.getMeanDistance(), 2)); + (*doc)[F("var")] = serialized(String(filter.getVariance(), 2)); + (*doc)[F("ci")] = serialized(String(1.959 * std::sqrt(filter.getVariance()) / std::sqrt(12), 2)); - float fusedMean = weightedMeans / sumWeights; - (*doc)[F("mean")] = serialized(String(fusedMean, 2)); - - float fusedVariance = weightedVariances / sumWeights; - (*doc)[F("var")] = serialized(String(fusedVariance, 2)); - - float fusedCI = weightedCIs / sumWeights; - (*doc)[F("ci")] = serialized(String(fusedCI, 2)); - - auto minRaw = std::min_element(channels.begin(), channels.end(), [](const BleChannelObservation& a, const BleChannelObservation& b) { return a.raw < b.raw; })->raw; - (*doc)[F("raw")] = serialized(String(minRaw, 2)); if (close) (*doc)[F("close")] = true; (*doc)[F("int")] = (millis() - firstSeenMillis) / seenCount; @@ -523,19 +489,19 @@ bool BleFingerprint::report(JsonObject *doc) { if (ignore || idType <= ID_TYPE_RAND_MAC || hidden) return false; if (reported) return false; - auto minObservedDistance = getMinObservedDistance(); + auto distance = getDistance(); auto maxDistance = BleFingerprintCollection::maxDistance; - if (maxDistance > 0 && minObservedDistance > maxDistance) + if (maxDistance > 0 && distance > maxDistance) return false; auto now = millis(); - if ((abs(minObservedDistance - lastReported) < BleFingerprintCollection::skipDistance) && (lastReportedMillis > 0) && (now - lastReportedMillis < BleFingerprintCollection::skipMs)) + if ((abs(distance - lastReported) < BleFingerprintCollection::skipDistance) && (lastReportedMillis > 0) && (now - lastReportedMillis < BleFingerprintCollection::skipMs)) return false; if (fill(doc)) { lastReportedMillis = now; - lastReported = minObservedDistance; + lastReported = distance; reported = true; return true; } @@ -597,16 +563,16 @@ bool BleFingerprint::shouldCount() { close = false; } - auto minObservedDistance = getMinObservedDistance(); + auto distance = getDistance(); bool prevCounting = counting; if (ignore || !countable) counting = false; else if (getMsSinceLastSeen() > BleFingerprintCollection::countMs) counting = false; - else if (counting && minObservedDistance > BleFingerprintCollection::countExit) + else if (counting && distance > BleFingerprintCollection::countExit) counting = false; - else if (!counting && minObservedDistance <= BleFingerprintCollection::countEnter) + else if (!counting && distance <= BleFingerprintCollection::countEnter) counting = true; if (prevCounting != counting) { diff --git a/lib/BleFingerprint/BleFingerprint.h b/lib/BleFingerprint/BleFingerprint.h index 3e40478..81c3c78 100644 --- a/lib/BleFingerprint/BleFingerprint.h +++ b/lib/BleFingerprint/BleFingerprint.h @@ -81,16 +81,9 @@ enum class BleChannel { struct BleChannelObservation { int rssi { NO_RSSI }; float raw { 0 }; - float dist { 0 }; - float mean { 0 }; - float vari { 0 }; - float ci { 0 }; unsigned long lastSeenMillis { 0 }; void observe(unsigned long timestamp, int rssi1m, int rssi); - -private: - FilteredDistance filter { ONE_EURO_FCMIN, ONE_EURO_BETA, ONE_EURO_DCUTOFF }; }; class BleFingerprint { @@ -119,9 +112,7 @@ class BleFingerprint { const short getIdType() const { return idType; } - const float getMinObservedDistance() const { - return std::min_element(channels.begin(), channels.end(), [](const BleChannelObservation& a, const BleChannelObservation& b) { return a.dist < b.dist; })->dist; - } + const float getDistance() const { return filter.getDistance(); } const int getMaxObservedRssi() const { return std::max_element(channels.begin(), channels.end(), [](const BleChannelObservation& a, const BleChannelObservation& b) { return a.rssi < b.rssi; })->rssi; @@ -169,7 +160,7 @@ class BleFingerprint { NimBLEAddress address; String id, name; short int idType = NO_ID_TYPE; - uint8_t lastChannel; + uint8_t lastChannel { 37 }; std::array channels; int8_t calRssi = NO_RSSI, bcnRssi = NO_RSSI, mdRssi = NO_RSSI, asRssi = NO_RSSI; unsigned int qryAttempts = 0, qryDelayMillis = 0; @@ -179,6 +170,7 @@ class BleFingerprint { uint16_t mv = 0; uint8_t battery = 0xFF, addressType = 0xFF; std::unique_ptr queryReport = nullptr; + FilteredDistance filter { ONE_EURO_FCMIN, ONE_EURO_BETA, ONE_EURO_DCUTOFF }; static bool shouldHide(const String &s); void fingerprint(NimBLEAdvertisedDevice *advertisedDevice); diff --git a/src/GUI.cpp b/src/GUI.cpp index 725ad5c..7a7f338 100644 --- a/src/GUI.cpp +++ b/src/GUI.cpp @@ -106,9 +106,9 @@ void Connected(bool wifi, bool mqtt) { void Counting(BleFingerprint *f, bool add) { if (add) - Serial.printf("\u001b[36m%u C# +1 | %s | %-58s%ddBm (%.2fm) %lums\u001b[0m\r\n", xPortGetCoreID(), f->getMac().c_str(), f->getId().c_str(), f->getMaxObservedRssi(), f->getMinObservedDistance(), f->getMsSinceLastSeen()); + Serial.printf("\u001b[36m%u C# +1 | %s | %-58s%ddBm (%.2fm) %lums\u001b[0m\r\n", xPortGetCoreID(), f->getMac().c_str(), f->getId().c_str(), f->getMaxObservedRssi(), f->getDistance(), f->getMsSinceLastSeen()); else - Serial.printf("\u001b[35m%u C# -1 | %s | %-58s%ddBm (%.2fm) %lums\u001b[0m\r\n", xPortGetCoreID(), f->getMac().c_str(), f->getId().c_str(), f->getMaxObservedRssi(), f->getMinObservedDistance(), f->getMsSinceLastSeen()); + Serial.printf("\u001b[35m%u C# -1 | %s | %-58s%ddBm (%.2fm) %lums\u001b[0m\r\n", xPortGetCoreID(), f->getMac().c_str(), f->getId().c_str(), f->getMaxObservedRssi(), f->getDistance(), f->getMsSinceLastSeen()); } void Wifi(unsigned int percent) {