MirrorSave
/
ESPresense
mirror of https://github.com/ESPresense/ESPresense.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Lots of filtering and tweaking

This commit is contained in:
DTTerastar 2021-03-22 18:56:29 -04:00
parent efb1fcf4a6
commit 5f2c508653
5 changed files with 57 additions and 89 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
lib/BleFingerprint/BleFingerprint.cpp
View File

@ -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;
}

15
lib/BleFingerprint/BleFingerprint.h
View File

@ -7,8 +7,7 @@
#include <NimBLEEddystoneTLM.h>
#include <NimBLEBeacon.h>
#include <ArduinoJson.h>
#include <kalman.h>
#include <SoftFilters.h>
class BleFingerprint
{
@ -26,8 +25,16 @@ public:
private:
String id;
int rssi, calRssi;
float original;
KalmanFilter kf;
float raw;
Reading<Differential<float>> output;
FilterChain compositeFilter;
TimestampFilter<float> tsFilter;
MovingAverageFilter<float, float> maFilter{MovingAverageFilter<float, float>(3)};
WeightedUpdateFilter<float, float> wuFilter{WeightedUpdateFilter<float, float>(0.1)};
OneEuroFilter<float, unsigned long> oneEuro{OneEuroFilter<float, unsigned long>(1, 1, 0.007, 1)};
DifferentialFilter<float> diffFilter;
StaticJsonDocument<500> doc;
unsigned long lastReading = 0;
};

10
platformio.ini
View File

@ -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

6
src/Settings.h
View File

@ -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

76
src/main.cpp
View File

@ -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)