From bc6f6402ebe91cdecb5ff7644734d3af1837f9e2 Mon Sep 17 00:00:00 2001 From: Maxim Slipenko Date: Sun, 3 Dec 2023 22:19:57 +0300 Subject: [PATCH] wip --- .gitignore | 2 +- components/esp32_nimble_mqtt_room/__init__.py | 39 ++++++++ .../esp32_nimble_mqtt_room.cpp | 20 ++++ .../esp32_nimble_mqtt_room.h | 24 +++++ components/esp32_nimble_tracker/__init__.py | 30 ++++++ .../esp32_nimble_tracker.cpp} | 0 .../esp32_nimble_tracker.h | 12 +++ .../FilterList.h | 0 .../OneEuro.h | 0 .../SoftFilters.h | 0 .../Tree.h | 0 components/nimble_distance_custom/__init__.py | 21 +++++ .../framework.h | 0 .../nimble_distance_custom.cpp | 91 +++++++++++++++++++ .../nimble_distance_custom.h} | 12 ++- .../types.h | 0 .../nimble_tracker/nimble_device_listener.cpp | 8 +- .../nimble_tracker/nimble_device_listener.h | 4 +- nimble_distance/__init__.py | 0 .../nimble_distance_sensor.cpp | 0 nimble_distance/nimble_distance_sensor.h | 31 +++++++ .../sensor.py | 2 +- 22 files changed, 286 insertions(+), 10 deletions(-) create mode 100644 components/esp32_nimble_mqtt_room/__init__.py create mode 100644 components/esp32_nimble_mqtt_room/esp32_nimble_mqtt_room.cpp create mode 100644 components/esp32_nimble_mqtt_room/esp32_nimble_mqtt_room.h create mode 100644 components/esp32_nimble_tracker/__init__.py rename components/{nimble_distance/__init__.py => esp32_nimble_tracker/esp32_nimble_tracker.cpp} (100%) create mode 100644 components/esp32_nimble_tracker/esp32_nimble_tracker.h rename components/{nimble_distance => nimble_distance_custom}/FilterList.h (100%) rename components/{nimble_distance => nimble_distance_custom}/OneEuro.h (100%) rename components/{nimble_distance => nimble_distance_custom}/SoftFilters.h (100%) rename components/{nimble_distance => nimble_distance_custom}/Tree.h (100%) create mode 100644 components/nimble_distance_custom/__init__.py rename components/{nimble_distance => nimble_distance_custom}/framework.h (100%) create mode 100644 components/nimble_distance_custom/nimble_distance_custom.cpp rename components/{nimble_distance/nimble_distance_sensor.h => nimble_distance_custom/nimble_distance_custom.h} (85%) rename components/{nimble_distance => nimble_distance_custom}/types.h (100%) create mode 100644 nimble_distance/__init__.py rename {components/nimble_distance => nimble_distance}/nimble_distance_sensor.cpp (100%) create mode 100644 nimble_distance/nimble_distance_sensor.h rename {components/nimble_distance => nimble_distance}/sensor.py (95%) diff --git a/.gitignore b/.gitignore index 086f413..cc427ae 100644 --- a/.gitignore +++ b/.gitignore @@ -5,4 +5,4 @@ /secrets.yaml /config.yaml /configa.yaml -__pycache__/** \ No newline at end of file +**/__pycache__/ \ No newline at end of file diff --git a/components/esp32_nimble_mqtt_room/__init__.py b/components/esp32_nimble_mqtt_room/__init__.py new file mode 100644 index 0000000..0ddc6b6 --- /dev/null +++ b/components/esp32_nimble_mqtt_room/__init__.py @@ -0,0 +1,39 @@ +import esphome.config_validation as cv +import esphome.codegen as cg +from esphome.const import ( + CONF_ID, +) +from esphome.components import mqtt, nimble_tracker + +DEPENDENCIES = ["mqtt"] +AUTO_LOAD=["nimble_distance_custom"] + +CONF_NIMBLE_ID = "esp32_nimble_mqtt_room" + +CONF_ROOM_KEY = 'room' +CONF_BASE_TOPIC_KEY = 'base_topic' +# CONF_MY_OPTIONAL_KEY = 'my_optional_key' +CONF_MAC_KEY = 'mac_addr' + +esp32_nimble_tracker_ns = cg.esphome_ns.namespace("esp32_nimble_mqtt_room") +ESP32NimbleMQTTRoom = esp32_nimble_tracker_ns.class_( + "ESP32NimbleMQTTRoom", cg.Component, nimble_tracker.NimbleDeviceListener +) + +CONFIG_SCHEMA = cv.Schema({ + cv.GenerateID(): cv.declare_id(ESP32NimbleMQTTRoom), + cv.Required(CONF_ROOM_KEY): cv.string, + cv.Required(CONF_MAC_KEY): cv.All(cv.ensure_list(cv.string)), + cv.Optional(CONF_BASE_TOPIC_KEY, default="esphome_presense"): cv.string, +}).extend(cv.COMPONENT_SCHEMA).extend(nimble_tracker.NIMBLE_DEVICE_LISTENER_SCHEMA) + +async def to_code(config): + var = cg.new_Pvariable(config[CONF_ID]) + await cg.register_component(var, config) + + cg.add(var.set_room(config[CONF_ROOM_KEY])) + cg.add(var.set_base_topic(config[CONF_BASE_TOPIC_KEY])) + cg.add(var.set_addresses(config[CONF_MAC_KEY])) + + await nimble_tracker.device_listener_to_code(var, config) + await nimble_tracker.register_ble_device(var, config) \ No newline at end of file diff --git a/components/esp32_nimble_mqtt_room/esp32_nimble_mqtt_room.cpp b/components/esp32_nimble_mqtt_room/esp32_nimble_mqtt_room.cpp new file mode 100644 index 0000000..e149b4a --- /dev/null +++ b/components/esp32_nimble_mqtt_room/esp32_nimble_mqtt_room.cpp @@ -0,0 +1,20 @@ +#include "esp32_nimble_mqtt_room.h" + +namespace esphome +{ + namespace esp32_nimble_mqtt_room + { + void ESP32NimbleMQTTRoom::on_result(nimble_distance_custom::NimbleDistanceCustomResult& result) + { + auto address = result.address.toString(); + + this->publish_json( + this->base_topic_ + "/devices/" + address + "/" + this->room_, + [=](ArduinoJson::JsonObject root) -> void { + root["id"] = address; + root["distance"] = result.distance; + } + ); + }; + } // namespace esp32_nimble_tracker +} // namespace esphome \ No newline at end of file diff --git a/components/esp32_nimble_mqtt_room/esp32_nimble_mqtt_room.h b/components/esp32_nimble_mqtt_room/esp32_nimble_mqtt_room.h new file mode 100644 index 0000000..d90d576 --- /dev/null +++ b/components/esp32_nimble_mqtt_room/esp32_nimble_mqtt_room.h @@ -0,0 +1,24 @@ +#pragma once + +#include "esphome/core/component.h" +#include "esphome/components/mqtt/custom_mqtt_device.h" +#include "esphome/components/nimble_distance_custom/nimble_distance_custom.h" + +namespace esphome +{ + namespace esp32_nimble_mqtt_room + { + class ESP32NimbleMQTTRoom : + public mqtt::CustomMQTTDevice, + public nimble_distance_custom::NimbleDistanceCustomComponent + { + protected: + std::string room_; + std::string base_topic_ = "esphome_presense"; + public: + void on_result(nimble_distance_custom::NimbleDistanceCustomResult&) override; + void set_room(std::string room) { room_ = room; } + void set_base_topic(std::string base_topic) { base_topic_ = base_topic; } + }; + } // namespace esp32_nimble_tracker +} // namespace esphome \ No newline at end of file diff --git a/components/esp32_nimble_tracker/__init__.py b/components/esp32_nimble_tracker/__init__.py new file mode 100644 index 0000000..4522150 --- /dev/null +++ b/components/esp32_nimble_tracker/__init__.py @@ -0,0 +1,30 @@ +import esphome.config_validation as cv +import esphome.codegen as cg +from esphome.const import ( + CONF_ID, +) + +DEPENDENCIES = ["esp32"] + +# CONF_NIMBLE_ID = "esp32_nimble_tracker" + +CONF_MY_REQUIRED_KEY = 'my_required_key' +CONF_MY_OPTIONAL_KEY = 'my_optional_key' + +esp32_nimble_tracker_ns = cg.esphome_ns.namespace("esp32_nimble_tracker") +ESP32NimbleTracker = esp32_nimble_tracker_ns.class_( + "ESP32NimbleTracker", cg.Component +) + +CONFIG_SCHEMA = cv.Schema({ + cv.GenerateID(): cv.declare_id(ESP32NimbleTracker), + cv.Required(CONF_MY_REQUIRED_KEY): cv.string, + cv.Optional(CONF_MY_OPTIONAL_KEY, default=10): cv.int_, +}).extend(cv.COMPONENT_SCHEMA) + +def to_code(config): + var = cg.new_Pvariable(config[CONF_ID]) + yield cg.register_component(var, config) + + await nimble_tracker.device_listener_to_code(var, config) + await nimble_tracker.register_ble_device(var, config) \ No newline at end of file diff --git a/components/nimble_distance/__init__.py b/components/esp32_nimble_tracker/esp32_nimble_tracker.cpp similarity index 100% rename from components/nimble_distance/__init__.py rename to components/esp32_nimble_tracker/esp32_nimble_tracker.cpp diff --git a/components/esp32_nimble_tracker/esp32_nimble_tracker.h b/components/esp32_nimble_tracker/esp32_nimble_tracker.h new file mode 100644 index 0000000..2ce6a33 --- /dev/null +++ b/components/esp32_nimble_tracker/esp32_nimble_tracker.h @@ -0,0 +1,12 @@ +#pragma once + +#include "esphome/core/component.h" +#include "esphome/components/sensor/sensor.h" + +namespace esphome +{ + namespace esp32_nimble_tracker + { + class ESP32NimbleTracker : public Component {}; + } // namespace esp32_nimble_tracker +} // namespace esphome \ No newline at end of file diff --git a/components/nimble_distance/FilterList.h b/components/nimble_distance_custom/FilterList.h similarity index 100% rename from components/nimble_distance/FilterList.h rename to components/nimble_distance_custom/FilterList.h diff --git a/components/nimble_distance/OneEuro.h b/components/nimble_distance_custom/OneEuro.h similarity index 100% rename from components/nimble_distance/OneEuro.h rename to components/nimble_distance_custom/OneEuro.h diff --git a/components/nimble_distance/SoftFilters.h b/components/nimble_distance_custom/SoftFilters.h similarity index 100% rename from components/nimble_distance/SoftFilters.h rename to components/nimble_distance_custom/SoftFilters.h diff --git a/components/nimble_distance/Tree.h b/components/nimble_distance_custom/Tree.h similarity index 100% rename from components/nimble_distance/Tree.h rename to components/nimble_distance_custom/Tree.h diff --git a/components/nimble_distance_custom/__init__.py b/components/nimble_distance_custom/__init__.py new file mode 100644 index 0000000..0eb7233 --- /dev/null +++ b/components/nimble_distance_custom/__init__.py @@ -0,0 +1,21 @@ +import esphome.codegen as cg +import esphome.config_validation as cv +from esphome.components import nimble_tracker +from esphome.const import ( + DEVICE_CLASS_DISTANCE, + STATE_CLASS_MEASUREMENT, + UNIT_METER, +) + +DEPENDENCIES = ["nimble_tracker"] + +nimble_custom_distance_ns = cg.esphome_ns.namespace("nimble_custom_distance") +NimbleDistanceCustomComponent = nimble_custom_distance_ns.class_( + "NimbleDistanceCustomComponent", + cg.Component, + nimble_tracker.NimbleDeviceListener, +) + +CONFIG_SCHEMA = cv.Schema({ + cv.GenerateID(): cv.declare_id(NimbleDistanceCustomComponent), +}).extend(cv.COMPONENT_SCHEMA).extend(nimble_tracker.NIMBLE_DEVICE_LISTENER_SCHEMA) diff --git a/components/nimble_distance/framework.h b/components/nimble_distance_custom/framework.h similarity index 100% rename from components/nimble_distance/framework.h rename to components/nimble_distance_custom/framework.h diff --git a/components/nimble_distance_custom/nimble_distance_custom.cpp b/components/nimble_distance_custom/nimble_distance_custom.cpp new file mode 100644 index 0000000..e1bf49a --- /dev/null +++ b/components/nimble_distance_custom/nimble_distance_custom.cpp @@ -0,0 +1,91 @@ +#include "nimble_distance_custom.h" + +namespace esphome +{ + namespace nimble_distance_custom + { + static const char *const TAG = "nimble_distance_custom"; + + static int median_of_3(int a, int b, int c) + { + int the_max = std::max(std::max(a, b), c); + int the_min = std::min(std::min(a, b), c); + // unnecessarily clever code + int the_median = the_max ^ the_min ^ a ^ b ^ c; + return (the_median); + } + int NimbleDistanceCustomComponent::get_1m_rssi(nimble_tracker::NimbleTrackerEvent *tracker_event) + { + return this->ref_rssi_; //+ tracker_event->getTXPower() + 99; + } + + Filter::Filter(float fcmin, float beta, float dcutoff) : one_euro_{OneEuroFilter(1, fcmin, beta, dcutoff)} + { + } + + bool Filter::filter(float rssi) + { + Reading inter1, inter2; + // TODO: should we take into consideration micro seconds (returned from esp_timer_get_time()) + // vs mili seconds (implementation used in ESPresence?) + inter1.timestamp = esp_timer_get_time(); + inter1.value = rssi; + + return this->one_euro_.push(&inter1, &inter2) && this->diff_filter_.push(&inter2, &this->output); + } + + void NimbleDistanceCustomComponent::setup() + { + this->filter_ = new Filter(ONE_EURO_FCMIN, ONE_EURO_BETA, ONE_EURO_DCUTOFF); + } + + // Defined distance formula using + // https://medium.com/beingcoders/convert-rssi-value-of-the-ble-bluetooth-low-energy-beacons-to-meters-63259f307283 + // and copied a lot of code from + // https://github.com/ESPresense/ESPresense/blob/master/lib/BleFingerprint/BleFingerprint.cpp + bool NimbleDistanceCustomComponent::update_state(nimble_tracker::NimbleTrackerEvent *tracker_event) + { + this->oldest_ = this->recent_; + this->recent_ = this->newest_; + this->newest_ = tracker_event->getRSSI(); + this->rssi_ = median_of_3(this->oldest_, this->recent_, this->newest_); + + float ratio = (this->get_1m_rssi(tracker_event) - this->rssi_) / (10.0f * this->absorption_); + float raw = std::pow(10, ratio); + + if (!this->filter_->filter(raw)) + { + ESP_LOGD(TAG, "Not enough data to calculate distance."); + return false; + } + + auto max_distance = 16.0f; + if (max_distance > 0 && this->filter_->output.value.position > max_distance) + return false; + + auto skip_distance = 0.5f; + auto skip_ms = 5000; + auto skip_micro_seconds = skip_ms * 1000; + auto now = esp_timer_get_time(); + + if ((abs(this->filter_->output.value.position - this->last_reported_position_) < skip_distance) && (this->last_reported_micro_seconds_ > 0) && ((now - this->last_reported_micro_seconds_) < skip_micro_seconds)) + { + return false; + } + + this->last_reported_micro_seconds_ = now; + this->last_reported_position_ = this->filter_->output.value.position; + // /this->publish_state(this->filter_->output.value.position); + + auto res = NimbleDistanceCustomResult{ + tracker_event->getAddress(), + this->filter_->output.value.position + }; + + this->on_result(res); + + return true; + } + } // namespace nimble_distance + +} // namespace esphome \ No newline at end of file diff --git a/components/nimble_distance/nimble_distance_sensor.h b/components/nimble_distance_custom/nimble_distance_custom.h similarity index 85% rename from components/nimble_distance/nimble_distance_sensor.h rename to components/nimble_distance_custom/nimble_distance_custom.h index f6d28eb..cabf128 100644 --- a/components/nimble_distance/nimble_distance_sensor.h +++ b/components/nimble_distance_custom/nimble_distance_custom.h @@ -18,12 +18,11 @@ // For NimbleDistanceSensor #include "esphome/core/component.h" -#include "esphome/components/sensor/sensor.h" #include "esphome/components/nimble_tracker/nimble_tracker.h" namespace esphome { - namespace nimble_distance + namespace nimble_distance_custom { class Filter { @@ -37,7 +36,13 @@ namespace esphome DifferentialFilter diff_filter_; }; - class NimbleDistanceSensor : public sensor::Sensor, + typedef struct + { + NimBLEAddress address; + float distance; + } NimbleDistanceCustomResult; + + class NimbleDistanceCustomComponent: public Component, public nimble_tracker::NimbleDeviceListener { @@ -47,6 +52,7 @@ namespace esphome protected: bool update_state(nimble_tracker::NimbleTrackerEvent *tracker_event) override; + virtual void on_result(NimbleDistanceCustomResult& result); Filter *filter_; int rssi_ = NO_RSSI, newest_ = NO_RSSI, recent_ = NO_RSSI, oldest_ = NO_RSSI; diff --git a/components/nimble_distance/types.h b/components/nimble_distance_custom/types.h similarity index 100% rename from components/nimble_distance/types.h rename to components/nimble_distance_custom/types.h diff --git a/components/nimble_tracker/nimble_device_listener.cpp b/components/nimble_tracker/nimble_device_listener.cpp index 85eb551..466fdd8 100644 --- a/components/nimble_tracker/nimble_device_listener.cpp +++ b/components/nimble_tracker/nimble_device_listener.cpp @@ -19,9 +19,9 @@ namespace esphome } } - void NimbleDeviceListener::set_address(std::string address) { + void NimbleDeviceListener::set_addresses(std::vector addresses) { this->match_by_ = MATCH_BY_ADDRESS; - this->address_ = address; + this->addresses_ = addresses; } bool NimbleDeviceListener::parse_event(NimbleTrackerEvent *tracker_event) @@ -51,7 +51,9 @@ namespace esphome ESP_LOGD(TAG, "Found device %s", tracker_event->toString().c_str()); auto address = tracker_event->getAddress().toString(); - if (this->address_ == address) { + auto &v = this->addresses_; + + if(std::find(v.begin(), v.end(), address) != v.end()) { return this->update_state(tracker_event); } else { return false; diff --git a/components/nimble_tracker/nimble_device_listener.h b/components/nimble_tracker/nimble_device_listener.h index b5c1fc2..279713b 100644 --- a/components/nimble_tracker/nimble_device_listener.h +++ b/components/nimble_tracker/nimble_device_listener.h @@ -14,7 +14,7 @@ namespace esphome public: bool parse_event(NimbleTrackerEvent *tracker_event); void set_irk(std::string irk_hex); - void set_address(std::string address); + void set_addresses(std::vector); protected: virtual bool update_state(NimbleTrackerEvent *tracker_event) = 0; @@ -27,7 +27,7 @@ namespace esphome MatchType match_by_; uint8_t *irk_; - std::string address_; + std::vector addresses_; }; } // namespace nimble_tracker diff --git a/nimble_distance/__init__.py b/nimble_distance/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/components/nimble_distance/nimble_distance_sensor.cpp b/nimble_distance/nimble_distance_sensor.cpp similarity index 100% rename from components/nimble_distance/nimble_distance_sensor.cpp rename to nimble_distance/nimble_distance_sensor.cpp diff --git a/nimble_distance/nimble_distance_sensor.h b/nimble_distance/nimble_distance_sensor.h new file mode 100644 index 0000000..3a2fa1f --- /dev/null +++ b/nimble_distance/nimble_distance_sensor.h @@ -0,0 +1,31 @@ +#pragma once + +// For Filter +#include +#include "esp_timer.h" +#include "SoftFilters.h" +// #define ONE_EURO_FCMIN 1e-5f +// #define ONE_EURO_BETA 1e-7f +// #define ONE_EURO_DCUTOFF 1e-5f + +// From https://github.com/rpatel3001/BleDistance/blob/master/ble_dist.h +#define ONE_EURO_FCMIN 0.0001 +#define ONE_EURO_BETA 0.05 +#define ONE_EURO_DCUTOFF 1.0 + +#define NO_RSSI (-128) +#define DEFAULT_TX (-6) + +// For NimbleDistanceSensor +#include "esphome/core/component.h" +#include "esphome/components/sensor/sensor.h" +#include "esphome.h" + +namespace esphome +{ + namespace nimble_distance + { + class NimbleDistanceSensor + : public sensor::Sensor, + public nimble_tracker::NimbleDistanceCustomComponent {} +} // namespace esphome \ No newline at end of file diff --git a/components/nimble_distance/sensor.py b/nimble_distance/sensor.py similarity index 95% rename from components/nimble_distance/sensor.py rename to nimble_distance/sensor.py index 8cdc07e..e0f632b 100644 --- a/components/nimble_distance/sensor.py +++ b/nimble_distance/sensor.py @@ -7,7 +7,7 @@ from esphome.const import ( UNIT_METER, ) -DEPENDENCIES = ["nimble_tracker"] +# DEPENDENCIES = ["nimble_custom_component"] nimble_distance_ns = cg.esphome_ns.namespace("nimble_distance") NimbleDistanceSensor = nimble_distance_ns.class_(