From 6bc92348aef0682689557e49fd228b8242ca6345 Mon Sep 17 00:00:00 2001 From: DTTerastar Date: Tue, 6 Apr 2021 20:13:13 -0400 Subject: [PATCH] Adding a median filter to rssi --- lib/BleFingerprint/BleFingerprint.cpp | 88 ++++++++++----------------- lib/BleFingerprint/BleFingerprint.h | 13 +++- lib/BleFingerprint/util.h | 50 +++++++++++++++ src/main.cpp | 3 +- 4 files changed, 93 insertions(+), 61 deletions(-) create mode 100644 lib/BleFingerprint/util.h diff --git a/lib/BleFingerprint/BleFingerprint.cpp b/lib/BleFingerprint/BleFingerprint.cpp index 9087668..238c14b 100644 --- a/lib/BleFingerprint/BleFingerprint.cpp +++ b/lib/BleFingerprint/BleFingerprint.cpp @@ -1,42 +1,5 @@ #include "BleFingerprint.h" - -static const uint16_t beaconUUID = 0xFEAA; -static const uint16_t tileUUID = 0xFEED; -static const uint16_t exposureUUID = 0xFD6F; - -#ifdef TX_DEFAULT -static const int defaultTxPower = TX_DEFAULT; -#else -static const int defaultTxPower = -59; -#endif - -#define ENDIAN_CHANGE_U16(x) ((((x)&0xFF00) >> 8) + (((x)&0xFF) << 8)) -#define Sprintf(f, ...) ({ char* s; asprintf(&s, f, __VA_ARGS__); String r = s; free(s); r; }) -//define SDateTimef(f) ({ struct tm firstSeenTm; gmtime_r(&f, &firstSeenTm); Sprintf("%d/%d/%d %d:%.2d:%.2d", firstSeenTm.tm_mon, firstSeenTm.tm_mday, 1900 + firstSeenTm.tm_year, firstSeenTm.tm_hour, firstSeenTm.tm_min, firstSeenTm.tm_sec); }) -#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]); }) - -static String getProximityUUIDString(BLEBeacon beacon) -{ - std::string serviceData = beacon.getProximityUUID().toString().c_str(); - int serviceDataLength = serviceData.length(); - String returnedString = ""; - int i = serviceDataLength; - while (i > 0) - { - if (serviceData[i - 1] == '-') - { - i--; - } - char a = serviceData[i - 1]; - char b = serviceData[i - 2]; - returnedString += b; - returnedString += a; - - i -= 2; - } - - return returnedString; -} +#include "util.h" BleFingerprint::~BleFingerprint() { @@ -47,6 +10,7 @@ BleFingerprint::BleFingerprint(BLEAdvertisedDevice *advertisedDevice, float fcmi { firstSeenMicros = esp_timer_get_time(); address = advertisedDevice->getAddress(); + newest = recent = oldest = rssi = advertisedDevice->getRSSI(); String mac_address = SMacf(address); @@ -165,36 +129,46 @@ BleFingerprint::BleFingerprint(BLEAdvertisedDevice *advertisedDevice, float fcmi Serial.println(); } -void BleFingerprint::seen(BLEAdvertisedDevice *advertisedDevice) -{ - lastSeenMicros = esp_timer_get_time(); - - rssi = advertisedDevice->getRSSI(); - if (!calRssi) - calRssi = defaultTxPower; - - float ratio = (calRssi - rssi) / 35.0f; - raw = pow(10, ratio); - setDistance(raw); - reported = false; -} - -void BleFingerprint::setDistance(float distFl) +bool BleFingerprint::filter() { Reading inter1, inter2; - if (tsFilter.push(&distFl, &inter1)) + if (tsFilter.push(&raw, &inter1)) { inter2.timestamp = inter1.timestamp; inter2.value = oneEuro(inter1.value); if (diffFilter.push(&inter2, &output)) - hasValue = true; + return true; + } + return false; +} + +void BleFingerprint::seen(BLEAdvertisedDevice *advertisedDevice) +{ + lastSeenMicros = esp_timer_get_time(); + + oldest = recent; + recent = newest; + newest = advertisedDevice->getRSSI(); + rssi = median_of_3(oldest, recent, newest); + + if (!calRssi) calRssi = defaultTxPower; + + float ratio = (calRssi - rssi) / 35.0f; + raw = pow(10, ratio); + + if (filter()) + { + hasValue = true; + reported = false; } } -float BleFingerprint::getDistance() +void BleFingerprint::setInitial(int initalRssi, float initalDistance) { - return output.value.position; + newest = recent = oldest = rssi = initalRssi; + raw = initalDistance; + hasValue = filter() || filter(); } bool BleFingerprint::report(JsonDocument *doc, int maxDistance) diff --git a/lib/BleFingerprint/BleFingerprint.h b/lib/BleFingerprint/BleFingerprint.h index 921a4dc..f878143 100644 --- a/lib/BleFingerprint/BleFingerprint.h +++ b/lib/BleFingerprint/BleFingerprint.h @@ -21,8 +21,10 @@ public: String getId() { return id; } - float getDistance(); - void setDistance(float distFl); + float getDistance() { return output.value.position; } + int getRSSI() { return rssi; } + + void setInitial(int rssi, float distance); NimBLEAddress getAddress() { return address; } long getLastSeen() { return lastSeenMicros; }; @@ -31,7 +33,10 @@ private: bool hasValue = false, enroll = false, reported = false; NimBLEAddress address; String id, name, url; - int rssi, calRssi; + int rssi = -100, calRssi = 0; + int newest = -100; + int recent = -100; + int oldest = -100; float raw, lastReported = 0; long firstSeenMicros, lastSeenMicros = 0, lastReportedMicros = 0; @@ -40,5 +45,7 @@ private: TimestampFilter tsFilter; one_euro_filter oneEuro; DifferentialFilter diffFilter; + + bool filter(); }; #endif diff --git a/lib/BleFingerprint/util.h b/lib/BleFingerprint/util.h new file mode 100644 index 0000000..28160a9 --- /dev/null +++ b/lib/BleFingerprint/util.h @@ -0,0 +1,50 @@ +#include +#include +#include + +#define ENDIAN_CHANGE_U16(x) ((((x)&0xFF00) >> 8) + (((x)&0xFF) << 8)) +#define Sprintf(f, ...) ({ char* s; asprintf(&s, f, __VA_ARGS__); String r = s; free(s); r; }) +//define SDateTimef(f) ({ struct tm firstSeenTm; gmtime_r(&f, &firstSeenTm); Sprintf("%d/%d/%d %d:%.2d:%.2d", firstSeenTm.tm_mon, firstSeenTm.tm_mday, 1900 + firstSeenTm.tm_year, firstSeenTm.tm_hour, firstSeenTm.tm_min, firstSeenTm.tm_sec); }) +#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]); }) + +#ifdef TX_DEFAULT +static const int defaultTxPower = TX_DEFAULT; +#else +static const int defaultTxPower = -59; +#endif + +static const uint16_t beaconUUID = 0xFEAA; +static const uint16_t tileUUID = 0xFEED; +static const uint16_t exposureUUID = 0xFD6F; + +static int median_of_3(int a, int b, int c) +{ + int the_max = max(max(a, b), c); + int the_min = min(min(a, b), c); + // unnecessarily clever code + int the_median = the_max ^ the_min ^ a ^ b ^ c; + return (the_median); +} + +static String getProximityUUIDString(BLEBeacon beacon) +{ + std::string serviceData = beacon.getProximityUUID().toString().c_str(); + int serviceDataLength = serviceData.length(); + String returnedString = ""; + int i = serviceDataLength; + while (i > 0) + { + if (serviceData[i - 1] == '-') + { + i--; + } + char a = serviceData[i - 1]; + char b = serviceData[i - 2]; + returnedString += b; + returnedString += a; + + i -= 2; + } + + return returnedString; +} diff --git a/src/main.cpp b/src/main.cpp index e1efb86..936d2c4 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -15,7 +15,7 @@ BleFingerprint *getFingerprintInternal(BLEAdvertisedDevice *advertisedDevice) if (it2 != fingerprints.end()) { auto found = *it2; - created->setDistance(found->getDistance()); + created->setInitial(found->getRSSI(), found->getDistance()); } fingerprints.push_front(created); @@ -39,6 +39,7 @@ bool sendTelemetry(int totalSeen = -1, int totalReported = -1, int totalAdverts tele["hostname"] = WiFi.getHostname(); tele["uptime"] = getUptimeSeconds(); tele["firm"] = String(FIRMWARE); + tele["rssi"] = WiFi.RSSI(); #ifdef VERSION tele["ver"] = String(VERSION);