Add max_distance option

Closes #1

.gitignore

@@ -3,3 +3,6 @@
 # You can modify this file to suit your needs.
 /.esphome/
 /secrets.yaml
+/config.yaml
+/configa.yaml
+**/__pycache__/

components/esp32_nimble_mqtt_room/__init__.py

@@ -0,0 +1,41 @@
+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_MAC_KEY = 'mac_addr'
+CONF_MAX_DISTANCE = 'max_distance'
+
+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_MAX_DISTANCE, default=16.0): cv.float_,
+  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]))
+  cg.add(var.set_max_distance(config[CONF_MAX_DISTANCE]))
+
+  await nimble_tracker.device_listener_to_code(var, config)
+  await nimble_tracker.register_ble_device(var, config)

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

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

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)

components/nimble_distance_custom/nimble_distance_custom.cpp

@@ -0,0 +1,95 @@
+#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<float, unsigned long>(1, fcmin, beta, dcutoff)}
+        {
+        }
+
+        bool Filter::filter(float rssi)
+        {
+            Reading<float, unsigned long> 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);
+        }
+
+
+        void NimbleDistanceCustomComponent::set_max_distance(float max_distance) {
+            this->max_distance_ = max_distance;
+        }
+
+        // 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;
+            }
+
+            if (this->max_distance_ > 0 && this->filter_->output.value.position > this->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

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,23 +36,33 @@ namespace esphome
             DifferentialFilter<float, unsigned long> diff_filter_;
         };
 
-        class NimbleDistanceSensor : public sensor::Sensor,
+        typedef struct
+        {
+            NimBLEAddress address;
+            float distance;
+        } NimbleDistanceCustomResult;
+
+        class NimbleDistanceCustomComponent:
                                      public Component,
                                      public nimble_tracker::NimbleDeviceListener
         {
         public:
             void setup() override;
             int get_1m_rssi(nimble_tracker::NimbleTrackerEvent *tracker_event);
+            void set_max_distance(float);
 
         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;
 
-            int8_t ref_rssi_ = -65;
+            int8_t ref_rssi_ = -75;
             float absorption_ = 3.5f;
             float last_reported_position_ = 0;
             int64_t last_reported_micro_seconds_ = 0;
+
+            float max_distance_ = 16.0;
         };
     } // namespace nimble_distance
 } // namespace esphome

components/nimble_tracker/__init__.py

@@ -111,4 +111,4 @@ async def device_listener_to_code(var, config):
     if CONF_IRK in config:
         cg.add(var.set_irk(config[CONF_IRK]))
     if CONF_MAC in config:
-        cg.add(var.set_irk(config[CONF_MAC]))
+        cg.add(var.set_address(config[CONF_MAC]))

components/nimble_tracker/nimble_device_listener.cpp

@@ -19,13 +19,15 @@ namespace esphome
             }
         }
 
-        void NimbleDeviceListener::set_address(std::string address) {
+        void NimbleDeviceListener::set_addresses(std::vector<std::string> addresses) {
             this->match_by_ = MATCH_BY_ADDRESS;
-            this->address_ = address;
+            this->addresses_ = addresses;
         }
 
         bool NimbleDeviceListener::parse_event(NimbleTrackerEvent *tracker_event)
         {
+            // ESP_LOGD(TAG, "Found device %s", tracker_event->toString().c_str());
+            ESP_LOGD(TAG, "%d", this->match_by_);
             if (this->match_by_ == MATCH_BY_IRK) {
                 if (tracker_event->getAddressType() != BLE_ADDR_RANDOM)
                 {
@@ -44,15 +46,21 @@ namespace esphome
                 {
                     return false;
                 }
-            } else if (this->match_by_ == MATCH_BY_ADDRESS) {
-                auto address = tracker_event->getAddress();
+            }
+            if (this->match_by_ == MATCH_BY_ADDRESS) {
+                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;
                 }
             }
+
+            return false;
         };
     } // namespace nimble_tracker
 

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::stding address);
+            void set_addresses(std::vector<std::string>);
 
         protected:
             virtual bool update_state(NimbleTrackerEvent *tracker_event) = 0;
@@ -22,12 +22,12 @@ namespace esphome
             enum MatchType
             {
                 MATCH_BY_IRK,
-                MATCH_BY_ADDRESS;
+                MATCH_BY_ADDRESS,
             };
             MatchType match_by_;
 
             uint8_t *irk_;
-            std::string address_;
+            std::vector<std::string> addresses_;
         };
 
     } // namespace nimble_tracker

config.yaml

@@ -1,110 +0,0 @@
-substitutions:
-  device_name: "nimble-shelly"
-  entity_id: "nimble_shelly"
-  room_name: "office"
-
-mqtt:
-  broker: !secret mqtt_broker
-  username: esphome
-  password: !secret mqtt_password
-  discovery: false
-  id: mqtt_client
-
-external_components:
-  - source:
-      type: local
-      path: ./components
-
-nimble_tracker:
-  scan_parameters:
-    # window: 500ms
-    # interval: 1.2s
-
-    window: 100ms
-    interval: 100ms
-    active: false
-
-globals:
-  - id: room_topic
-    type: std::string
-    initial_value: '"room_presence/${room_name}"'
-
-sensor:
-  - platform: nimble_distance
-    irk: !secret apple_watch_irk
-    name: "Apple Watch Distance"
-    id: apple_watch_distance
-    internal: true
-    # TODO: should we retain the mqtt message?
-    on_value:
-      then:
-        - lambda: |-
-            id(mqtt_client)->publish_json(id(room_topic), [=](ArduinoJson::JsonObject root) -> void {
-              root["id"] = "apple_watch";
-              root["name"] = "Apple Watch";
-              root["distance"] = id(apple_watch_distance).state;
-            });
-
-esphome:
-  name: ${device_name}
-  platformio_options:
-    board_build.f_cpu: 160000000L
-
-esp32:
-  board: esp32dev
-  framework:
-    type: esp-idf
-    sdkconfig_options:
-      CONFIG_FREERTOS_UNICORE: y
-      CONFIG_ESP32_DEFAULT_CPU_FREQ_160: y
-      CONFIG_ESP32_DEFAULT_CPU_FREQ_MHZ: "160"
-      # From https://github.com/esphome/issues/issues/2941
-      # Increase watchdog timeout to flash firmware with bluetooth enabled, fixes error:
-      # ERROR Error receiving acknowledge binary size: timed out
-      CONFIG_ESP_TASK_WDT_TIMEOUT_S: "20"
-      CONFIG_BT_BLE_50_FEATURES_SUPPORTED: y
-      CONFIG_BT_BLE_42_FEATURES_SUPPORTED: y
-
-output:
-  - platform: gpio
-    id: "relay_output"
-    pin: GPIO26
-
-status_led:
-  pin:
-    number: GPIO0
-    inverted: true
-
-# Enable logging
-logger:
-  level: VERBOSE
-
-# Enable Home Assistant API
-api:
-  encryption:
-    key: !secret api_encryption_key
-
-ota:
-  password: !secret ota_password
-
-wifi:
-  ssid: !secret wifi_ssid
-  password: !secret wifi_password
-
-  # Enable fallback hotspot (captive portal) in case wifi connection fails
-  ap:
-    password: !secret wifi_password
-
-switch:
-  - platform: output
-    id: shelly_relay
-    name: "${entity_id}"
-    output: "relay_output"
-    # after reboot, keep the relay off. this prevents light turning on after a power outage
-    restore_mode: ALWAYS_OFF
-
-# From https://community.home-assistant.io/t/shelly-plus-1-esphome-bletracker/363549/38
-# Setup a button in home assistant to reboot the shelly into safe mode
-button:
-  - platform: safe_mode
-    name: "${entity_id}_safe_mode_restart"

nimble_distance/nimble_distance_sensor.cpp

@@ -16,7 +16,7 @@ namespace esphome
         }
         int NimbleDistanceSensor::get_1m_rssi(nimble_tracker::NimbleTrackerEvent *tracker_event)
         {
-            return this->ref_rssi_ + tracker_event->getTXPower();
+            return this->ref_rssi_; //+ tracker_event->getTXPower() + 99;
         }
 
         Filter::Filter(float fcmin, float beta, float dcutoff) : one_euro_{OneEuroFilter<float, unsigned long>(1, fcmin, beta, dcutoff)}

nimble_distance/nimble_distance_sensor.h

@@ -0,0 +1,31 @@
+#pragma once
+
+// For Filter
+#include <cstddef>
+#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

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_(