diff --git a/lib/BleFingerprint/BleFingerprint.cpp b/lib/BleFingerprint/BleFingerprint.cpp index f3a5290..5457309 100644 --- a/lib/BleFingerprint/BleFingerprint.cpp +++ b/lib/BleFingerprint/BleFingerprint.cpp @@ -49,10 +49,12 @@ StaticJsonDocument<500> BleFingerprint::getJson() BleFingerprint::BleFingerprint(BLEAdvertisedDevice *advertisedDevice, float initalDistance) { - kf.init(); - kf.setProcessNoise(0.1, 0.01); // Position, velocity - kf.setMeasurementNoise(1); - kf.set(initalDistance); + compositeFilter.append(&wuFilter); + compositeFilter.append(&maFilter); + compositeFilter.append(&tsFilter); + compositeFilter.append(&oneEuro); + compositeFilter.append(&diffFilter); + String mac_address = advertisedDevice->getAddress().toString().c_str(); mac_address.replace(":", ""); mac_address.toLowerCase(); @@ -191,28 +193,27 @@ void BleFingerprint::seen(BLEAdvertisedDevice *advertisedDevice) if (!calRssi) calRssi = defaultTxPower; - float dt = (millis() - lastReading) / 1000.f; - lastReading = millis(); - kf.predict(dt); - float ratio = (calRssi - rssi) / 25.0f; + float ratio = (calRssi - rssi) / 35.0f; float distFl = pow(10, ratio); - kf.correct(distFl); - original = round(distFl * 10) / 10; + + if (compositeFilter.push(&distFl, &output)) + { + raw = distFl; + // if (id == "2c96d71f47569faddd487c93cc9dac2e-0-0") + // Serial.printf("%-36s %5.1f %5.1f %5.1f %5.1f\n", id.c_str(), distFl, output.value.position, output.value.speed * 1e6, output.value.acceleration * 1e12); + } } void BleFingerprint::report(BLEAdvertisedDevice *advertisedDevice) { - float filtered = kf.get(); - float distance = round(filtered * 10) / 10; - - Serial.printf("%s", id.c_str()); - Serial.printf(", RSSI: %d (@1m %d)", rssi, calRssi); - Serial.printf(", Dist: %.1f (orig %.1f)", distance, original); - Serial.println(); + //Serial.printf("%s", id.c_str()); + //Serial.printf(", RSSI: %d (@1m %d)", rssi, calRssi); + //Serial.printf(", Dist: %.1f (orig %.1f)", distance, original); + //Serial.println(); doc["id"] = id; doc["rssi@1m"] = calRssi; doc["rssi"] = rssi; - doc["distance"] = distance; - doc["original"] = original; + doc["raw"] = round(raw * 100.0f) / 100.0f; + doc["distance"] = round(output.value.position * 100.0f) / 100.0f; } diff --git a/lib/BleFingerprint/BleFingerprint.h b/lib/BleFingerprint/BleFingerprint.h index 32a3577..cbe8ad7 100644 --- a/lib/BleFingerprint/BleFingerprint.h +++ b/lib/BleFingerprint/BleFingerprint.h @@ -7,8 +7,7 @@ #include #include #include - -#include +#include class BleFingerprint { @@ -26,8 +25,16 @@ public: private: String id; int rssi, calRssi; - float original; - KalmanFilter kf; + float raw; + Reading> output; + + FilterChain compositeFilter; + TimestampFilter tsFilter; + MovingAverageFilter maFilter{MovingAverageFilter(3)}; + WeightedUpdateFilter wuFilter{WeightedUpdateFilter(0.1)}; + OneEuroFilter oneEuro{OneEuroFilter(1, 1, 0.007, 1)}; + DifferentialFilter diffFilter; + StaticJsonDocument<500> doc; unsigned long lastReading = 0; }; diff --git a/platformio.ini b/platformio.ini index cef69db..9d6e8ae 100644 --- a/platformio.ini +++ b/platformio.ini @@ -23,7 +23,7 @@ lib_deps = AsyncTCP juerd/ESP-WiFiSettings@^3.7.2 bbx10/DNSServer@^1.1.0 - tobiasebsen/KalmanFilter@^1.0.0 + haimoz/SoftFilters@^0.1.0 lib_ignore = ESPAsyncTCP board_build.partitions = partitions_singleapp.csv monitor_speed = 115200 @@ -43,7 +43,7 @@ lib_deps = m5stack/M5StickC@^0.2.0 juerd/ESP-WiFiSettings@^3.7.2 bbx10/DNSServer@^1.1.0 - tobiasebsen/KalmanFilter@^1.0.0 + haimoz/SoftFilters@^0.1.0 lib_ignore = ESPAsyncTCP board_build.partitions = partitions_singleapp.csv monitor_speed = 115200 @@ -64,7 +64,7 @@ lib_deps = m5stack/M5StickCPlus@^0.0.1 juerd/ESP-WiFiSettings@^3.7.2 bbx10/DNSServer@^1.1.0 - tobiasebsen/KalmanFilter@^1.0.0 + haimoz/SoftFilters@^0.1.0 lib_ignore = ESPAsyncTCP board_build.partitions = partitions_singleapp.csv monitor_speed = 115200 @@ -85,7 +85,7 @@ lib_deps = AsyncTCP juerd/ESP-WiFiSettings@^3.7.2 bbx10/DNSServer@^1.1.0 - tobiasebsen/KalmanFilter@^1.0.0 + haimoz/SoftFilters@^0.1.0 lib_ignore = ESPAsyncTCP board_build.partitions = partitions_singleapp.csv monitor_speed = 115200 @@ -107,7 +107,7 @@ lib_deps = m5stack/M5StickC@^0.2.0 juerd/ESP-WiFiSettings@^3.7.2 bbx10/DNSServer@^1.1.0 - tobiasebsen/KalmanFilter@^1.0.0 + haimoz/SoftFilters@^0.1.0 lib_ignore = ESPAsyncTCP board_build.partitions = partitions_singleapp.csv monitor_speed = 115200 diff --git a/src/Settings.h b/src/Settings.h index a9fe930..7af71c1 100644 --- a/src/Settings.h +++ b/src/Settings.h @@ -44,9 +44,9 @@ // Define bluetooth scan parameters #define BLE_ACTIVE_SCAN true // Active scan uses more power, but get results faster -#define BLE_SCAN_DURATION 5 // Define the duration of a single scan in seconds -#define BLE_SCAN_INTERVAL 0x80 // Used to determine antenna sharing between Bluetooth and WiFi. Do not modify unless you are confident you know what you're doing -#define BLE_SCAN_WINDOW 0x79 // Used to determine antenna sharing between Bluetooth and WiFi. Do not modify unless you are confident you know what you're doing +#define BLE_SCAN_DURATION 2 // Define the duration of a single scan in seconds +#define BLE_SCAN_INTERVAL 333 // Used to determine antenna sharing between Bluetooth and WiFi. Do not modify unless you are confident you know what you're doing +#define BLE_SCAN_WINDOW 320 // Used to determine antenna sharing between Bluetooth and WiFi. Do not modify unless you are confident you know what you're doing // Maximum distance (in meters) to report. Devices that are calculated to be further than this distance in meters will not be reported #define MAX_DISTANCE 8 diff --git a/src/main.cpp b/src/main.cpp index 40595a3..c23a85c 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -70,7 +70,6 @@ TimerHandle_t wifiReconnectTimer; bool updateInProgress = false; String localIp; byte retryAttempts = 0; -unsigned long last = 0; BLEScan *pBLEScan; TaskHandle_t thBLEScan; @@ -175,32 +174,6 @@ unsigned long CalculateUptimeSeconds(void) return (0xFFFFFFFF / 1000) * _rolloverCount + (_lastMillis / 1000); } -#ifdef htuSensorTopic - -void reportSensorValues() -{ - if (htuSensorIsConnected()) - { - char temp[8]; - char humidity[8]; - - dtostrf(getTemp(), 0, 1, temp); // convert float to string with one decimal place precision - dtostrf(getHumidity(), 0, 1, humidity); // convert float to string with one decimal place precision - - if (mqttClient.publish(tempTopic, 0, 0, temp) == true) - { - Serial.printf("Temperature %s sent\t", temp); - } - - if (mqttClient.publish(humidityTopic, 0, 0, humidity) == true) - { - Serial.printf("Humidity %s sent\n\r", humidity); - } - } -} - -#endif - bool sendTelemetry(int deviceCount = -1, int reportCount = -1) { StaticJsonDocument<256> tele; @@ -216,16 +189,10 @@ bool sendTelemetry(int deviceCount = -1, int reportCount = -1) #endif if (deviceCount > -1) - { - Serial.printf("devices_discovered: %d\n\r", deviceCount); tele["disc_ct"] = deviceCount; - } if (reportCount > -1) - { - Serial.printf("devices_reported: %d\n\r", reportCount); tele["rept_ct"] = reportCount; - } char teleMessageBuffer[258]; serializeJson(tele, teleMessageBuffer); @@ -236,19 +203,18 @@ bool sendTelemetry(int deviceCount = -1, int reportCount = -1) if (mqttClient.publish((TELEMETRY_TOPIC).c_str(), 0, 0, teleMessageBuffer) == true) { - Serial.println("Telemetry sent"); return true; } else { - Serial.println("Error sending telemetry"); + log_e("Error sending telemetry"); return false; } } void connectToWifi() { - Serial.println("Connecting to WiFi..."); + log_i("Connecting to WiFi..."); pinMode(LED_BUILTIN, OUTPUT); // Set custom callback functions @@ -283,6 +249,9 @@ void connectToWifi() Serial.println(WiFi.localIP()); Serial.print("Hostname: \t"); Serial.println(WiFi.getHostname()); + Serial.print("Room: \t"); + Serial.println(room); + localIp = WiFi.localIP().toString(); } @@ -291,7 +260,7 @@ void connectToMqtt() { if (WiFi.isConnected() && !updateInProgress) { - Serial.println("Connecting to MQTT"); + log_i("Connecting to MQTT"); mqttClient.setCredentials(mqttUser.c_str(), mqttPass.c_str()); mqttClient.setClientId(hostname.c_str()); mqttClient.connect(); @@ -305,7 +274,7 @@ void handleMqttDisconnect() if (retryAttempts > 10) { - Serial.println("Too many mqtt retries. Restarting"); + log_e("Too many mqtt retries. Restarting"); WiFiSettings.portal(); } else @@ -315,7 +284,7 @@ void handleMqttDisconnect() if (WiFi.isConnected() && !updateInProgress) { - Serial.println("Starting MQTT reconnect timer"); + log_i("Starting MQTT reconnect timer"); xTimerReset(mqttReconnectTimer, 0); } } @@ -324,12 +293,12 @@ void handleWifiDisconnect() { if (WiFi.isConnected()) { - Serial.println("WiFi appears to be connected. Not retrying."); + log_i("WiFi appears to be connected. Not retrying."); return; } if (retryAttempts > 10) { - Serial.println("Too many retries. Restarting"); + log_e("Too many retries. Restarting"); ESP.restart(); } else @@ -347,17 +316,16 @@ void handleWifiDisconnect() void onMqttConnect(bool sessionPresent) { - Serial.println("Connected to MQTT"); + log_i("Connected to MQTT"); retryAttempts = 0; if (mqttClient.publish((AVAILABILITY_TOPIC).c_str(), 0, 1, "CONNECTED") == true) { - Serial.print("Success sending message to topic:\t"); - Serial.println(AVAILABILITY_TOPIC); + log_d("Success sending message to topic: %s", AVAILABILITY_TOPIC); } else { - Serial.println("Error sending message"); + log_e("Error sending message"); } sendTelemetry(); @@ -443,14 +411,11 @@ void scanForDevices(void *parameter) i++; if (!updateInProgress) { - pBLEScan->setActiveScan(i % 4 == 0 ? BLE_ACTIVE_SCAN : false); - if (i % 4 == 0) - Serial.print("Scanning (ACTIVE)...\t"); - else - Serial.print("Scanning...\t"); + pBLEScan->setActiveScan(i % 10 == 0 ? BLE_ACTIVE_SCAN : false); + pBLEScan->clearResults(); + BLEScanResults foundDevices = pBLEScan->start(BLE_SCAN_DURATION); int devicesCount = foundDevices.getCount(); - Serial.printf("Scan done! Devices found: %d\n\r", devicesCount); #ifdef M5STICK M5.Lcd.setCursor(0, 0); @@ -470,11 +435,10 @@ void scanForDevices(void *parameter) } } sendTelemetry(devicesCount, devicesReported); - pBLEScan->clearResults(); } else { - Serial.println("Cannot report; mqtt disconnected"); + log_e("Cannot report; mqtt disconnected"); if (xTimerIsTimerActive(mqttReconnectTimer) != pdFALSE) { TickType_t xRemainingTime = xTimerGetExpiryTime(mqttReconnectTimer) - xTaskGetTickCount(); @@ -486,7 +450,6 @@ void scanForDevices(void *parameter) handleMqttDisconnect(); } } - last = millis(); } } } @@ -534,7 +497,6 @@ void setClock() { configTime(0, 0, "pool.ntp.org", "time.nist.gov"); // UTC - Serial.print(F("Waiting for NTP time sync: ")); time_t now = time(nullptr); while (now < 8 * 3600 * 2) { @@ -543,11 +505,9 @@ void setClock() now = time(nullptr); } - Serial.println(F("")); struct tm timeinfo; gmtime_r(&now, &timeinfo); - Serial.print(F("Current time: ")); - Serial.print(asctime(&timeinfo)); + log_i(F("NTP synced, current time: %s"), asctime(&timeinfo)); } void firmwareUpdate(void)