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

Kalman filter added, better distance calculation, m5stickC support added

This commit is contained in:
DTTerastar 2021-03-13 15:20:16 -05:00
parent 4c8b84ab08
commit 9e4e85e430
3 changed files with 624 additions and 509 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
.editorconfig Normal file
View File

@ -0,0 +1,8 @@
[*]
end_of_line = lf
insert_final_newline = true
# 4 spaces indentation
[*.ino]
indent_style = space
indent_size = 4

13
platformio.ini
View File

@ -6,16 +6,19 @@
; Advanced options: extra scripting
;
; Please visit documentation for the other options and examples
; http://docs.platformio.org/page/projectconf.html
[platformio]
src_dir = ./src
; https://docs.platformio.org/page/projectconf.html
[env:esp32]
platform = espressif32
framework = arduino
board = m5stick-c
lib_deps = ArduinoJson@^6, NimBLE-Arduino, AsyncMqttClient@^0.8.2, AsyncTCP
lib_deps =
ArduinoJson@^6
NimBLE-Arduino
AsyncMqttClient@^0.8.2
AsyncTCP
dwrobel/TrivialKalmanFilter@^1.0.1
m5stack/M5StickC@^0.2.0
lib_ignore = ESPAsyncTCP, ESP32 BLE Arduino
board_build.partitions = partitions_singleapp.csv
monitor_speed = 115200

198
src/ESP32-mqtt-room.ino
View File

@ -37,6 +37,9 @@ extern "C"
#include "Settings.h"
#include <TrivialKalmanFilter.h>
#include <M5StickC.h>
#ifdef htuSensorTopic
#define tempTopic htuSensorTopic "/temperature"
#define humidityTopic htuSensorTopic "/humidity"
@ -44,15 +47,20 @@ extern "C"
#endif
static const int scanTime = singleScanTime;
static const int waitTime = scanInterval;
static const uint16_t beaconUUID = 0xFEAA;
static const uint16_t tileUUID = 0xFEED;
static const uint16_t exposureUUID = 0xFD6F;
#ifdef TxDefault
static const int defaultTxPower = TxDefault;
#else
static const int defaultTxPower = -40;
static const int defaultTxPower = -59;
#endif
#define ENDIAN_CHANGE_U16(x) ((((x)&0xFF00) >> 8) + (((x)&0xFF) << 8))
#define MAX_MAC_ADDRESSES 50
#define DT_COVARIANCE_RK 2
#define DT_COVARIANCE_QK 0.1
WiFiClient espClient;
AsyncMqttClient mqttClient;
TimerHandle_t mqttReconnectTimer;
@ -63,6 +71,67 @@ byte retryAttempts = 0;
unsigned long last = 0;
BLEScan *pBLEScan;
TaskHandle_t BLEScan;
TrivialKalmanFilter<float> *filters[MAX_MAC_ADDRESSES];
int mac_pos(const uint8_t mac[6])
{
static uint8_t mac_lib[MAX_MAC_ADDRESSES][6];
static unsigned int mac_count = 0;
for (int i = 0; i < mac_count && i < MAX_MAC_ADDRESSES; i++)
{
bool flag = true;
for (uint8_t j = 0; j < 6; j++)
{
if (mac_lib[i][j] != mac[j])
{
flag = false;
break;
}
}
if (flag == true)
return i;
}
uint8_t dest = mac_count % MAX_MAC_ADDRESSES;
for (uint8_t j = 0; j < 6; j++)
mac_lib[dest][j] = mac[j];
mac_count++;
return dest;
}
/**
* CalculateUptimeSeconds()
*
* Handle millis() rollover and calculate the total uptime in seconds.
* This function must be called at least once for every 50 days to be
* able to see the rollover.
*/
unsigned long CalculateUptimeSeconds(void)
{
static unsigned int _rolloverCount = 0; // Number of 0xFFFFFFFF rollover we had in millis()
static unsigned long _lastMillis = 0; // Value of the last millis()
// Get the current milliseconds uptime from the system.
// Note: This only works as long as no one else did hook up with timer0
// because the arduino system uses timer0 to manage delay() and millis().
unsigned long currentMilliSeconds = millis();
// If we had a rollover we count that.
if (currentMilliSeconds < _lastMillis)
{
_rolloverCount++;
}
// Now store the current number of milliseconds for the next round.
_lastMillis = currentMilliSeconds;
// Based on the current milliseconds and the number of rollovers
// we had in total we calculate here the uptime in seconds since
// poweron or reset.
// Caution: Because we shorten millis to seconds we may miss one
// second for every rollover (1 second every 50 days).
return (0xFFFFFFFF / 1000) * _rolloverCount + (_lastMillis / 1000);
}
String getProximityUUIDString(BLEBeacon beacon)
{
@ -87,25 +156,30 @@ String getProximityUUIDString(BLEBeacon beacon)
return returnedString;
}
float calcDistance(float rssi, float calRssi)
void initFilters()
{
for (int i = 0; i < MAX_MAC_ADDRESSES; i++)
filters[i] = new TrivialKalmanFilter<float>(DT_COVARIANCE_RK, DT_COVARIANCE_QK);
}
void calcDistance(int pos, int rssi, int calRssi, StaticJsonDocument<500> *doc)
{
if (rssi == 0)
return -1.0;
if (!calRssi)
calRssi = defaultTxPower;
float ratio = (calRssi - rssi) / 35;
float ratio = (calRssi - rssi) / 25.0f;
float distFl = pow(10, ratio);
float distance = round(distFl * 100) / 100;
float filtered = filters[pos]->update(distFl);
float distance = round(filtered * 10) / 10;
float original = round(distFl * 10) / 10;
Serial.print(", RSSI@1m: ");
Serial.print(calRssi);
Serial.print(", RSSI: ");
Serial.print(rssi);
Serial.print(", distance: ");
Serial.print(distance);
Serial.printf(", RSSI: %d (@1m %d)", rssi, calRssi);
Serial.printf(", Dist: %.1f (orig %.1f)", distance, original);
return distance;
(*doc)["rssi@1m"] = calRssi;
(*doc)["rssi"] = rssi;
(*doc)["distance"] = distance;
(*doc)["original"] = original;
}
#ifdef htuSensorTopic
@ -141,8 +215,8 @@ bool sendTelemetry(int deviceCount = -1, int reportCount = -1)
tele["ip"] = localIp;
tele["hostname"] = WiFi.getHostname();
tele["scan_dur"] = scanTime;
tele["wait_dur"] = waitTime;
tele["max_dist"] = maxDistance;
tele["uptime"] = CalculateUptimeSeconds();
if (deviceCount > -1)
{
@ -360,7 +434,6 @@ bool reportDevice(BLEAdvertisedDevice advertisedDevice)
mac_address.toLowerCase();
//Check scanned MAC Address against a list of allowed MAC Addresses
if (allowedListCheck)
{
bool allowedListFound = false;
@ -379,6 +452,9 @@ bool reportDevice(BLEAdvertisedDevice advertisedDevice)
}
// --------------
int pos = mac_pos(advertisedDevice.getAddress().getNative());
//Serial.printf("%d ", pos);
Serial.print("MAC: ");
Serial.print(mac_address);
int rssi = advertisedDevice.getRSSI();
@ -390,14 +466,9 @@ bool reportDevice(BLEAdvertisedDevice advertisedDevice)
Serial.print(nameBLE);
doc["name"] = nameBLE;
}
else
{
doc["name"] = mac_address;
}
doc["id"] = mac_address;
doc["mac"] = mac_address;
doc["rssi"] = rssi;
std::string strServiceData = advertisedDevice.getServiceData();
uint8_t cServiceData[100];
@ -407,22 +478,28 @@ bool reportDevice(BLEAdvertisedDevice advertisedDevice)
{
for (int i = 0; i < advertisedDevice.getServiceUUIDCount(); i++)
{
Serial.printf(", sID: %s", advertisedDevice.getServiceUUID(i).toString().c_str());
std::string sid = advertisedDevice.getServiceUUID(i).toString();
Serial.printf(", sID: %s", sid.c_str());
doc["sid" + String(i)] = sid;
}
}
if (advertisedDevice.haveServiceUUID() && advertisedDevice.getServiceDataUUID().equals(BLEUUID("0xFEED")) == true)
if (advertisedDevice.haveServiceUUID() && advertisedDevice.getServiceDataUUID().equals(BLEUUID(tileUUID)) == true)
{
Serial.print(", Tile");
doc["name"] = "Tile";
if (advertisedDevice.haveTXPower())
doc["txPower"] = advertisedDevice.getTXPower();
doc["distance"] = calcDistance(rssi, advertisedDevice.haveTXPower() ? -advertisedDevice.getTXPower() - 41 : 0);
calcDistance(pos, rssi, advertisedDevice.haveTXPower() ? (-advertisedDevice.getTXPower()) - 41 : 0, &doc);
}
else if (advertisedDevice.haveServiceUUID() && advertisedDevice.getServiceDataUUID().equals(BLEUUID(exposureUUID)) == true)
{ // found covid exposure tracker
Serial.print(", Exposure");
doc["id"] = "exp:" + String(strServiceData.length());
calcDistance(pos, rssi, advertisedDevice.haveTXPower() ? (-advertisedDevice.getTXPower()) - 41 : 0, &doc);
}
else if (advertisedDevice.haveServiceUUID() && advertisedDevice.getServiceDataUUID().equals(BLEUUID(beaconUUID)) == true)
{ // found Eddystone UUID
Serial.print(", Eddystone");
// Update distance variable for Eddystone BLE devices
// Update distance v ariable for Eddystone BLE devices
if (cServiceData[0] == 0x10)
{
BLEEddystoneURL oBeacon = BLEEddystoneURL();
@ -432,8 +509,7 @@ bool reportDevice(BLEAdvertisedDevice advertisedDevice)
doc["url"] = oBeacon.getDecodedURL().c_str();
Serial.print(" URL: ");
Serial.print(oBeacon.getDecodedURL().c_str());
doc["txPower"] = oBeacon.getPower();
doc["distance"] = calcDistance(rssi, oBeacon.getPower());
calcDistance(pos, rssi, oBeacon.getPower(), &doc);
}
else if (cServiceData[0] == 0x20)
{
@ -451,8 +527,11 @@ bool reportDevice(BLEAdvertisedDevice advertisedDevice)
uint8_t cManufacturerData[100];
strManufacturerData.copy((char *)cManufacturerData, strManufacturerData.length(), 0);
char *mdHex = NimBLEUtils::buildHexData(nullptr, (uint8_t *)strManufacturerData.data(), strManufacturerData.length());
if (strManufacturerData.length() == 25 && cManufacturerData[0] == 0x4C && cManufacturerData[1] == 0x00)
if (strManufacturerData.length() > 2 && cManufacturerData[0] == 0x4C && cManufacturerData[1] == 0x00) // Apple
{
if (strManufacturerData.length() == 25 && cManufacturerData[2] == 0x02 && cManufacturerData[3] == 0x15)
{
BLEBeacon oBeacon = BLEBeacon();
oBeacon.setData(strManufacturerData);
@ -470,25 +549,36 @@ bool reportDevice(BLEAdvertisedDevice advertisedDevice)
doc["minor"] = minor;
doc["id"] = proximityUUID + "-" + String(major) + "-" + String(minor);
doc["txPower"] = oBeacon.getSignalPower();
doc["distance"] = calcDistance(rssi, oBeacon.getSignalPower());
calcDistance(pos, rssi, oBeacon.getSignalPower(), &doc);
}
else
{
String fingerprint = "apple:" + String(mdHex).substring(4, 8) + ":" + String(strManufacturerData.length());
if (advertisedDevice.haveTXPower())
fingerprint = fingerprint + String(-advertisedDevice.getTXPower());
doc["id"] = fingerprint;
Serial.printf(", Fingerprint: %s", fingerprint.c_str());
calcDistance(pos, rssi, advertisedDevice.haveTXPower() ? (-advertisedDevice.getTXPower()) - 41 : 0, &doc);
}
}
else
{
if (advertisedDevice.haveTXPower())
doc["txPower"] = advertisedDevice.getTXPower();
doc["distance"] = calcDistance(rssi, advertisedDevice.haveTXPower() ? advertisedDevice.getTXPower() - 41 : 0);
doc["txPower"] = -advertisedDevice.getTXPower();
Serial.print(", MD: ");
for (int x = 0; x < strManufacturerData.length(); x++)
Serial.print(strManufacturerData[x], HEX);
calcDistance(pos, rssi, advertisedDevice.haveTXPower() ? (-advertisedDevice.getTXPower()) - 41 : 0, &doc);
}
free(mdHex);
}
else
{
if (advertisedDevice.haveTXPower())
doc["txPower"] = advertisedDevice.getTXPower();
doc["distance"] = calcDistance(rssi, advertisedDevice.haveTXPower() ? -advertisedDevice.getTXPower() - 41 : 0);
calcDistance(pos, rssi, advertisedDevice.haveTXPower() ? (-advertisedDevice.getTXPower()) - 41 : 0, &doc);
}
}
@ -543,29 +633,37 @@ bool reportDevice(BLEAdvertisedDevice advertisedDevice)
class MyAdvertisedDeviceCallbacks : public BLEAdvertisedDeviceCallbacks
{
void onResult(BLEAdvertisedDevice advertisedDevice)
void onResult(BLEAdvertisedDevice *advertisedDevice)
{
Serial.printf("Advertised Device: %s \n", advertisedDevice.toString().c_str());
digitalWrite(LED_BUILTIN, LED_ON);
vTaskDelay(10 / portTICK_PERIOD_MS);
//Serial.printf("Advertised Device: %s \n", advertisedDevice->toString().c_str());
vTaskDelay(advertisedDevice->getRSSI() > -60 ? 2 : 1);
digitalWrite(LED_BUILTIN, !LED_ON);
}
};
void scanForDevices(void *parameter)
{
int i = 0;
while (1)
{
if (!updateInProgress && WiFi.isConnected() && (millis() - last > (waitTime * 1000) || last == 0))
i++;
if (!updateInProgress)
{
pBLEScan->setActiveScan(i % 4 == 0);
if (i % 4 == 0)
Serial.print("Scanning (ACTIVE)...\t");
else
Serial.print("Scanning...\t");
BLEScanResults foundDevices = pBLEScan->start(scanTime);
int devicesCount = foundDevices.getCount();
Serial.printf("Scan done! Devices found: %d\n\r", devicesCount);
M5.Lcd.fillScreen(TFT_BLACK);
M5.Lcd.setTextDatum(MC_DATUM);
M5.Lcd.drawNumber(devicesCount, 40, 80, 7);
M5.Lcd.setTextDatum(MC_DATUM);
int devicesReported = 0;
if (mqttClient.connected())
{
@ -634,13 +732,12 @@ void configureOTA()
ESP.restart();
});
ArduinoOTA.setHostname(hostname);
ArduinoOTA.setPort(8266);
ArduinoOTA.setPort(3232);
ArduinoOTA.begin();
}
void setup()
{
Serial.begin(115200);
pinMode(LED_BUILTIN, OUTPUT);
@ -666,9 +763,11 @@ void setup()
configureOTA();
initFilters();
BLEDevice::init("");
pBLEScan = BLEDevice::getScan(); //create new scan
//pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks(), true, true);
pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks(), true);
pBLEScan->setActiveScan(activeScan);
pBLEScan->setInterval(bleScanInterval);
pBLEScan->setWindow(bleScanWindow);
@ -681,6 +780,11 @@ void setup()
1,
&BLEScan,
1);
M5.begin();
//M5.Lcd.setRotation(1);
M5.Lcd.fillScreen(TFT_BLACK);
M5.Lcd.setTextColor(TFT_WHITE, TFT_BLACK);
}
void loop()