first commit

.gitignore

@@ -0,0 +1,5 @@
+# Gitignore settings for ESPHome
+# This is an example and may include too much for your use-case.
+# You can modify this file to suit your needs.
+/.esphome/
+/secrets.yaml

components/nimble_distance/FilterList.h

@@ -0,0 +1,107 @@
+/**
+ * @file FilterList.h
+ * The linked list class to support the Tree class and FilterChain class.
+ * @note
+ * This implementation is not meant to be directly used by the client code.
+ *
+ * @see Tree, FilterChain
+ *
+ * @author Haimo Zhang <zh.hammer.dev@gmail.com>
+ */
+#ifndef FILTERLIST_H
+#define FILTERLIST_H
+
+template <typename VAL_T>
+class FilterNode
+{
+public:
+	VAL_T value;
+	FilterNode<VAL_T> *next;
+	FilterNode(VAL_T const &v): value(v), next(NULL) { }
+};
+
+template <typename VAL_T>
+class NodeIterator
+{
+public:
+	/**
+	 * Dereference the iterator to obtain the value it points at.
+	 */
+	VAL_T operator*() { return ptr->value; }
+	/**
+	 * Prefix increment operator.
+	 *
+	 * @note
+	 * Only the prefix operator __without__ return value is implemented.
+	 * Only use the standalone prefix increment statement, i.e.,
+	 * `++iter`.
+	 */
+	void operator++() { ptr = ptr->next; }
+	/**
+	 * Comparison between two iterators.
+	 *
+	 * @note
+	 * Only the `!=` operator is implemented, not the `==` operator.
+	 */
+	bool operator!=(NodeIterator<VAL_T> const &it) { return ptr != it.ptr; }
+	NodeIterator(): ptr(NULL) { }
+	NodeIterator(FilterNode<VAL_T> *n): ptr(n) { }
+private:
+	FilterNode<VAL_T> *ptr;
+};
+
+template <typename VAL_T>
+class FilterList
+{
+public:
+	FilterList(): head(NULL), tail(NULL), last_ptr(NULL) { }
+	~FilterList()
+	{
+		FilterNode<VAL_T> *to_del;
+		while (head) {
+			to_del = head;
+			head = head->next;
+			delete to_del;
+		}
+	}
+	/**
+	 * Append an element.  The value of the argument is copied.
+	 */
+	void append(VAL_T const &v)
+	{
+		FilterNode<VAL_T> *new_node = new FilterNode<VAL_T>(v);
+		if (head) {
+			*tail = new_node;
+		}
+		else {
+			head = new_node;
+		}
+		tail = &new_node->next;
+		last_ptr = new_node;
+	}
+	/**
+	 * Tell whether the linked list is empty.
+	 */
+	bool isEmpty() { return head == NULL; }
+	typedef NodeIterator<VAL_T> iterator;
+	/**
+	 * An iterator pointing at the beginning of the linked list.
+	 * Essentially pointing at the first node.
+	 */
+	NodeIterator<VAL_T> begin() { return NodeIterator<VAL_T>(head); }
+	/**
+	 * An iterator pointing at the end of the linked list.
+	 * Essentially pointing at NULL.
+	 */
+	NodeIterator<VAL_T> end() { return NodeIterator<VAL_T>(); }
+	/**
+	 * An iterator pointing at the last node.
+	 */
+	NodeIterator<VAL_T> last() { return NodeIterator<VAL_T>(last_ptr); }
+private:
+	FilterNode<VAL_T> *head;
+	FilterNode<VAL_T> **tail;
+	FilterNode<VAL_T> *last_ptr;
+};
+
+#endif

components/nimble_distance/OneEuro.h

@@ -0,0 +1,80 @@
+#ifndef ONEEURO_H
+#define ONEEURO_H
+/*
+1-Euro Filter, template-compliant version
+Jonathan Aceituno <join@oin.name>
+
+25/04/14: fixed bug with last_time_ never updated on line 40
+
+For details, see http://www.lifl.fr/~casiez/1euro
+
+Updates:
+
+- 23 May 2019 by Haimo Zhang <zh.hammer.dev@gmail.com>
+  - Included Arduino header
+
+*/
+
+#ifdef ARDUINO
+/**
+ * cmath is not included in Arduino library, but we have the Arduino.h header
+ * which defines the abs macro.
+ */
+#include <Arduino.h>
+#else
+#include <cmath>
+#endif
+
+template <typename T = double>
+struct low_pass_filter {
+	low_pass_filter() : hatxprev(0), xprev(0), hadprev(false) {}
+	T operator()(T x, T alpha) {
+		T hatx;
+		if(hadprev) {
+			hatx = alpha * x + (1-alpha) * hatxprev;
+		} else {
+			hatx = x;
+			hadprev = true;
+		}
+		hatxprev = hatx;
+		xprev = x;
+		return hatx;
+	}
+	T hatxprev;
+	T xprev;
+	bool hadprev;
+};
+
+template <typename T = double, typename timestamp_t = double>
+struct one_euro_filter {
+	one_euro_filter(double _freq, T _mincutoff, T _beta, T _dcutoff) : freq(_freq), mincutoff(_mincutoff), beta(_beta), dcutoff(_dcutoff), last_time_(-1) {}
+	T operator()(T x, timestamp_t t = -1) {
+		T dx = 0;
+		
+		if(last_time_ != -1 && t != -1 && t != last_time_) {
+			freq = 1.0 / (t - last_time_);
+		}
+		last_time_ = t;
+		
+		if(xfilt_.hadprev)
+			dx = (x - xfilt_.xprev) * freq;
+		
+		T edx = dxfilt_(dx, alpha(dcutoff));
+		T cutoff = mincutoff + beta * abs(static_cast<double>(edx));
+		return xfilt_(x, alpha(cutoff));
+	}
+	
+	double freq;
+	T mincutoff, beta, dcutoff;
+private:
+	T alpha(T cutoff) {
+		T tau = 1.0 / (2 * M_PI * cutoff);
+		T te = 1.0 / freq;
+		return 1.0 / (1.0 + tau / te);
+	}
+	
+	timestamp_t last_time_;
+	low_pass_filter<T> xfilt_, dxfilt_;
+};
+
+#endif

components/nimble_distance/SoftFilters.h

@@ -0,0 +1,496 @@
+#ifndef SOFTFILTERS_H
+#define SOFTFILTERS_H
+
+#include "framework.h"
+#include "types.h"
+#include "OneEuro.h"
+
+/**
+ * A differential filter calculates the speed and acceleration from its raw
+ * scalar input.
+ *
+ * Time chart of a data structure to support second and third order
+ * (speed & acceleration):
+ *
+ * ```text
+ *
+ *       +-- previous
+ *       | +-- before
+ *       | | +-- (current)
+ *       | | | +-- after
+ *       | | | | +-- next
+ *       | | | | |
+ * pos   * | * | *
+ *        \|/|\|/      spd = d_pos / d_t
+ * spd     *-+-*       interpolate two speeds to get the current speed
+ *          \|/        acc = d_spd / d_t
+ * acc       *
+ *
+ * ```
+ *
+ */
+template <typename VAL_T, typename TS_T=unsigned long, typename INTERNAL_T=double>
+class DifferentialFilter : public BaseFilter<Reading<VAL_T, TS_T>, Reading<Differential<VAL_T>, TS_T> >
+{
+
+public:
+	typedef Reading<VAL_T, TS_T> IN_T;
+	typedef Reading<Differential<VAL_T>, TS_T> OUT_T;
+	DifferentialFilter() : seen_first(false), seen_second(false) { }
+
+protected:
+
+/**
+ * Cast x to the internal processing data type.
+ */
+#define ITN(x) ((INTERNAL_T)(x))
+/**
+ * Interpolates the value of p1
+ * given positions and values of two other points p0 and p2.
+ *
+ * @note
+ * The parameters are evaluated more than once.
+ */
+#define INTERPOLATE(p0, v0, p1, p2, v2) (ITN(v2)*(ITN(p1)-ITN(p0))+ITN(v0)*(ITN(p2)-ITN(p1)))/(ITN(p2)-ITN(p0))
+	virtual bool update(void const * const input) override
+	{
+		in_ptr = (IN_T const * const) input;
+		if (!seen_first) {
+			// On the first observation, cache the value and timestamp
+			// in the internal storage for the output value.
+			this->out_val.value.position = in_ptr->value;
+			this->out_val.timestamp = in_ptr->timestamp;
+			seen_first = true;
+			return false;
+		}
+		else if (!seen_second) {
+			// Ignore the observation if the same timestamp,
+			// to avoid divide-by-zero.
+			if (this->out_val.timestamp != in_ptr->timestamp) {
+				// On the second observation, calculate the speed.
+				next_pos = in_ptr->value;
+				next_ts = in_ptr->timestamp;
+				aft_ts = ITN(next_ts - this->out_val.timestamp);
+				aft_spd = ITN(next_pos - this->out_val.value.position) / aft_ts;
+				aft_ts /= ITN(2);
+				aft_ts += ITN(this->out_val.timestamp);
+				seen_second = true;
+			}
+			return false;
+		}
+		else {
+			// Ignore the observation if the same timestamp,
+			// to avoid divide-by-zero.
+			if (next_ts == in_ptr->timestamp) {
+				return false;
+			}
+			// update internal data
+			bef_spd = aft_spd;
+			bef_ts = aft_ts;
+			this->out_val.value.position = next_pos;
+			this->out_val.timestamp = next_ts;
+			next_pos = in_ptr->value;
+			next_ts = in_ptr->timestamp;
+			// calculate the new speed
+			aft_ts = ITN(next_ts - this->out_val.timestamp);
+			aft_spd = ITN(next_pos - this->out_val.value.position) / aft_ts;
+			aft_ts /= ITN(2);
+			aft_ts += ITN(this->out_val.timestamp);
+			// interpolate the speed
+			this->out_val.value.speed = INTERPOLATE(bef_ts, bef_spd, this->out_val.timestamp, aft_ts, aft_spd);
+			// calculate the acceleration
+			this->out_val.value.acceleration = (aft_spd - bef_spd) / (aft_ts - bef_ts);
+			return true;
+		}
+	}
+#undef INTERPOLATE
+#undef ITN
+
+private:
+
+	INTERNAL_T bef_spd, bef_ts;
+	INTERNAL_T aft_spd, aft_ts;
+	VAL_T next_pos;
+	TS_T next_ts;
+	bool seen_first;
+	bool seen_second;
+	IN_T const * in_ptr;
+};
+
+/**
+ * A filter that adds timestamps to the input values.
+ *
+ * @tparam VAL_T type of the input values.
+ * @tparam TS_T type of the timestamp, defaults to `unsigned long`
+ * as per Arduino documentation of `millis` and `micros`.
+ * @tparam time_fn
+ * A function taking no parameter and returns a timestamp.
+ * On Arduino platforms, this defaults to the `micros` function.
+ * Otherwise (e.g., when used in other C++ environments), there is no default.
+ */
+#ifdef ARDUINO
+template <typename VAL_T, typename TS_T=unsigned long, TS_T (*time_fn)()=micros>
+#else
+template <typename VAL_T, typename TS_T=unsigned long, TS_T (*time_fn)()=esp_timer_get_time>
+#endif
+class TimestampFilter : public BaseFilter<VAL_T, Reading<VAL_T, TS_T> >
+{
+public:
+	virtual bool update(void const * const input) override
+	{
+		this->out_val.value = *((VAL_T const * const) input);
+		this->out_val.timestamp = time_fn();
+		return true;
+	}
+};
+
+/**
+ * A filter with a data cache, which is suitable for output that depends on
+ * several previous input data.
+ *
+ * This class is internally implemented as a circular buffer.
+ *
+ * @tparam IN_T input data type
+ * @tparam OUT_T output data type
+ */
+template <typename IN_T, typename OUT_T>
+class CachedFilter : public BaseFilter<IN_T, OUT_T>
+{
+public:
+	CachedFilter(size_t cap) : capacity(cap), size(0), end(0)
+	{
+		this->buffer = new IN_T[this->capacity];
+	}
+	~CachedFilter() { delete[] this->buffer; }
+protected:
+	virtual bool update(void const * const input) override
+	{
+		if (this->size < this->capacity) {
+			++size;
+			this->buffer[end++] = *(IN_T const * const) input;
+			end %= capacity;
+			return refresh((IN_T const * const) input, NULL, this->out_val);
+		}
+		else {
+			cached_val = this->buffer[end];
+			this->buffer[end++] = *(IN_T const * const) input;
+			end %= capacity;
+			return refresh((IN_T const * const) input, &cached_val, this->out_val);
+		}
+	}
+	/**
+	 * Refresh the output value given the new value added to the cache
+	 * and the old value removed from the cache.
+	 *
+	 * @note
+	 * To be differentiated from the CachedFilter::update member function
+	 * which overrides the BaseFilter::update member function.
+	 */
+	virtual bool refresh(IN_T const * const new_val, IN_T const * const old_val, OUT_T &output) = 0;
+	size_t get_capacity() { return capacity; }
+	size_t get_size() { return size; }
+private:
+	size_t capacity;  ///< The cache capacity, i.e., maximum data it can hold.
+	size_t size;  ///< The current cache size, i.e., valid data.
+	IN_T *buffer;  ///< The internal buffer that holds the cached data.
+	/**
+	 * The position in the internal buffer that points to the end of the cache.
+	 * New data will be written at this position and this value will be
+	 * incremented, wrapping around at the boundary of the internal buffer.
+	 * When the cache is full, this position points at the oldest data which
+	 * will be overwritten by the next incoming data.
+	 */
+	int end;
+	IN_T cached_val;  ///< used to temporarily store the old data before overwritten by the new data
+};
+
+/**
+ * A filter that outputs the average of a moving window.
+ *
+ * @tparam IN_T Input data type.
+ * @tparam OUT_T Output data type.
+ * @tparam INTERNAL_T The type used for internal processing.
+ * Defaults to `double`.
+ * Any input is first cast to the internal type for processing,
+ * whose result is then cast to the output type.
+ */
+template <typename IN_T, typename OUT_T, typename INTERNAL_T=double>
+class MovingAverageFilter : public CachedFilter<IN_T, OUT_T>
+{
+public:
+	/**
+	 * Create a moving average filter with the specified window size.
+	 *
+	 * @param [in] w_sz The window size.
+	 */
+	MovingAverageFilter(size_t w_sz) : CachedFilter<IN_T, OUT_T>(w_sz), sum(0) {  }
+protected:
+	INTERNAL_T get_sum() { return sum; }
+	virtual bool refresh(IN_T const * const new_val, IN_T const * const old_val, OUT_T &output) override
+	{
+		// Udpate the sum.
+		sum += (INTERNAL_T) *new_val - (old_val == NULL ? 0 : (INTERNAL_T) *old_val);
+		output = internal_result = sum / (INTERNAL_T) this->get_size();
+		return true;
+	}
+	INTERNAL_T internal_result;  ///< representing the result in internal type in case the output type does not have the required precision
+private:
+	INTERNAL_T sum;  ///< sum of the current cache content
+};
+
+/**
+ * A moving variance filter.
+ */
+template <typename IN_T, typename OUT_T, typename INTERNAL_T=double>
+class MovingVarianceFilter : public MovingAverageFilter<IN_T, OUT_T, INTERNAL_T>
+{
+public:
+	MovingVarianceFilter(size_t w_sz) : MovingAverageFilter<IN_T, OUT_T, INTERNAL_T>(w_sz), squared_sum(0) { }
+protected:
+	INTERNAL_T get_squared_sum() { return squared_sum; }
+	virtual bool refresh(IN_T const * const new_val, IN_T const * const old_val, OUT_T &output) override
+	{
+		// now the `internal_result` holds the mean value
+		MovingAverageFilter<IN_T, OUT_T>::refresh(new_val, old_val, output);
+		new_val_2 = *new_val;
+		new_val_2 *= new_val_2;
+		old_val_2 = old_val == NULL ? 0 : *old_val;
+		old_val_2 *= old_val_2;
+		squared_sum += new_val_2 - old_val_2;
+		output = this->internal_result = squared_sum / (INTERNAL_T) this->get_size() - this->internal_result * this->internal_result;
+		return true;
+	}
+private:
+	INTERNAL_T new_val_2;  ///< square of the new data
+	INTERNAL_T old_val_2;  ///< square of the old data
+	INTERNAL_T squared_sum;  ///< squared sum
+};
+
+/**
+ * A filter that updates the output based on a weighted average between its
+ * previous output and the current input.
+ *
+ * Mathematically, let \f$w\f$ be the sensitivity (weight of the input),
+ * \f$1-w\f$ be the innertia (weight of the previous output),
+ * \f$u_i\f$ be the \f$i\f$-th input, and \f$v_i\f$ be the \f$i\f$-th output.
+ * Then
+ * \f{aligned}{
+ * v_0 &= u_0 \\
+ * v_i &= w \, u_i + (1 - w) \, v_{i-1} \;\;\;\;\text{for}\; i>0, w\in[0,1]
+ * \f}
+ */
+template <typename IN_T, typename OUT_T, typename INTERNAL_T=double>
+class WeightedUpdateFilter : public BaseFilter<IN_T, OUT_T>
+{
+public:
+	WeightedUpdateFilter(double w) : sensitivity(w), innertia(1 - w), seen_first(false) { }
+	virtual bool update(void const * const input) override
+	{
+		if (seen_first) {
+			this->out_val = internal_result = innertia * internal_result + sensitivity * (INTERNAL_T) *(IN_T const * const)input;
+		}
+		else {
+			this->out_val = internal_result = *(IN_T const * const)input;
+		}
+		return true;
+	}
+protected:
+	INTERNAL_T internal_result;  ///< representing the result in internal type in case the output type does not have the required precision
+private:
+	INTERNAL_T sensitivity;  // sensitivity of the new observation, as a factor between 0 and 1
+	INTERNAL_T innertia;  // 1 - sensitivity
+	bool seen_first;
+};
+
+/**
+ * An adaptive normalization filter.
+ * It outputs a double-precision floating point number
+ * with value between 0 to 1,
+ * normalized against the range of all previous input to this filter.
+ */
+template <typename VAL_T>
+class AdaptiveNormalizationFilter: public BaseFilter<VAL_T, double>
+{
+public:
+	AdaptiveNormalizationFilter() : seen_first(false) { }
+	virtual bool update(void const * const input) override
+	{
+		in_val = *(VAL_T const * const) input;
+		if (!seen_first) {
+			maximum = minimum = in_val;
+			this->out_val = 0;
+			seen_first = true;
+		}
+		else {
+			if (in_val >= maximum) {
+				maximum = in_val;
+				this->out_val = 1;
+			}
+			else if (in_val <= minimum) {
+				minimum = in_val;
+				this->out_val = 0;
+			}
+			else {
+				this->out_val = (in_val - minimum) / (double) (maximum - minimum);
+			}
+		}
+		return true;
+	}
+private:
+	VAL_T maximum;
+	VAL_T minimum;
+	VAL_T in_val;
+	bool seen_first;
+};
+
+/**
+ * The 1-euro filter is based on the paper of the same name by Gery Casiez
+ */
+template <typename VAL_T, typename TS_T>
+class OneEuroFilter : public BaseFilter<Reading<VAL_T, TS_T>, Reading<VAL_T, TS_T>>
+{
+public:
+	OneEuroFilter(double _freq, VAL_T _mincutoff, VAL_T _beta, VAL_T _dcutoff)
+		: filter(one_euro_filter<VAL_T, TS_T>(_freq, _mincutoff, _beta, _dcutoff))
+	{}
+	VAL_T mincutoff()
+	{
+		return filter.mincutoff;
+	}
+	VAL_T mincutoff(VAL_T v)
+	{
+		filter.mincutoff = v;
+		return v;
+	}
+	VAL_T beta()
+	{
+		return filter.beta;
+	}
+	VAL_T beta(VAL_T v)
+	{
+		filter.beta = v;
+		return v;
+	}
+	VAL_T dcutoff()
+	{
+		return filter.dcutoff;
+	}
+	VAL_T dcutoff(VAL_T v)
+	{
+		filter.dcutoff = v;
+		return v;
+	}
+protected:
+	virtual bool update(void const * const input) override
+	{
+		this->out_val.value = filter(
+				((Reading<VAL_T, TS_T> const * const)input)->value,
+				((Reading<VAL_T, TS_T> const * const)input)->timestamp);
+		this->out_val.timestamp = ((Reading<VAL_T, TS_T> const * const)input)->timestamp;
+		return true;
+	}
+private:
+	one_euro_filter<VAL_T, TS_T> filter;
+};
+
+/**
+ * A lambda filter that uses a client-supplied filter function.
+ *
+ * @tparam IN_T type of input data
+ * @tparam OUT_T type of output data
+ */
+template <typename IN_T, typename OUT_T>
+class LambdaFilter : public BaseFilter<IN_T, OUT_T>
+{
+public:
+	/**
+	 * Create a lambda filter using the given function.
+	 *
+	 * @param f
+	 * A function with the same signature as the BaseFilter::push function.
+	 */
+	LambdaFilter(bool (*f)(IN_T const &, OUT_T &)): lambda(f) { }
+protected:
+	/**
+	 * In a lambda filter,
+	 * the update function simply calls the client-supplied filter function.
+	 */
+	virtual bool update(void const * const input) override
+	{
+		return lambda(*(IN_T const * const) input, this->out_val);
+	}
+private:
+	bool (*lambda)(IN_T const &, OUT_T &);
+};
+
+/**
+ * A flow rate filter measures the flow rate of incoming data.
+ *
+ * @tparam T The type of the data that is passing through.
+ * @tparam TS_T
+ *         The type of timestamp.
+ *         Defaults to `unsigned long` as per documentation of the Arduino
+ *         `millis` and `micros` functions.
+ * @tparam time_fun
+ *         A timestamp function that takes no input parameter
+ *         and returns a timestamp in the unit of number of ticks.
+ * @tparam TICKS_PER_SEC
+ *         Number of ticks (unit of the timestamp) per second.
+ */
+#ifdef ARDUINO
+template <typename T, typename TS_T=unsigned long, TS_T (*time_fun)()=micros, TS_T TICKS_PER_SEC=(int)1e6>
+#else
+template <typename T, typename TS_T, TS_T (*time_fun)(), TS_T TICKS_PER_SEC>
+#endif
+class FlowRateFilter : public PassThroughFilter<T>
+{
+public:
+	FlowRateFilter() : PassThroughFilter<T>(), total_count(0), first_ts(0), last_ts(0) { }
+	virtual bool update(void const * const input) override
+	{
+		PassThroughFilter<T>::update(input);
+		if (++total_count == 1) {
+			first_ts = last_ts = time_fun();
+		}
+		else {
+			last_ts = time_fun();
+		}
+		return true;
+	}
+	/**
+	 * Calculate the data rate in "frames per second".
+	 *
+	 * Essentially the "framerate" is calculated as the total number of data
+	 * devided by the total duration in seconds
+	 * since arrival of the first data.
+	 *
+	 * @returns The data per second in double precision.
+	 */
+	double get_flow_rate()
+	{
+		if (first_ts == last_ts) {
+			// return an invalid value since the flow rate cannot be calculated
+			return -1;
+		}
+		return total_count / get_duration_in_seconds();
+	}
+	/**
+	 * @returns The total number of data that have passed through this filter.
+	 */
+	unsigned long get_count() { return total_count; }
+	/**
+	 * @returns The total number of clock "ticks" (platform-dependent)
+	 * since arrival of the first data.
+	 */
+	TS_T get_duration_in_ticks() { return last_ts - first_ts; }
+	/**
+	 * @returns The total number of seconds since arrival of the first data.
+	 */
+	double get_duration_in_seconds() { return get_duration_in_ticks() / (double) TICKS_PER_SEC; }
+private:
+	unsigned long total_count;  ///< total data count
+	TS_T first_ts;  ///< timestamp of first data
+	TS_T last_ts;  ///< timestamp of latest data
+};
+
+#endif

components/nimble_distance/Tree.h

@@ -0,0 +1,31 @@
+/**
+ * @todo
+ * Implement and document the Tree class.
+ */
+
+/**
+ * @file Tree.h
+ * The tree class to support the filter framework.
+ * @note
+ * This implementation is not meant to be directly used by the client code.
+ *
+ * @see framework.h
+ *
+ * @author Haimo Zhang <zh.hammer.dev@gmail.com>
+ */
+#ifndef TREE_H
+#define TREE_H
+
+#include "FilterList.h"
+
+template <typename VAL_T>
+class Tree
+{
+public:
+	VAL_T value;
+	void appendChild(Tree<VAL_T> const &child) { subtrees.append(&child); }
+protected:
+	FilterList<Tree<VAL_T> *> subtrees;
+};
+
+#endif

components/nimble_distance/framework.h

@@ -0,0 +1,230 @@
+/**
+ * @file framework.h
+ * %Filter framework.
+ *
+ * @author Haimo Zhang <zh.hammer.dev@gmail.com>
+ */
+#ifndef FRAMEWORK_H
+#define FRAMEWORK_H
+
+#include "FilterList.h"
+#include "Tree.h"
+
+/**
+ * The %Filter interface without type checking at compile time.
+ *
+ * @note
+ * It is the client code's responsibility to ensure
+ * that the correct pointers are passed to the filter.
+ */
+class Filter
+{
+	// Composite filter classes need to access protected and private members
+	// of Filter instances, and are thus marked as friends.
+	friend class FilterChain;
+	friend class FilterTree;
+public:
+	/**
+	 * Push a new data through the filter.
+	 *
+	 * A filter is not required to always output a data in response
+	 * to a new input data.
+	 * For example, a delay filter might wait for several input data
+	 * before outputing.
+	 * This behavior is supported through the boolean return value.
+	 *
+	 * @note
+	 * The input and output memory is managed by the client code,
+	 * i.e., the client code is responsible for
+	 * the lifetime of the input and output memory
+	 * and the validity of the two pointers.
+	 *
+	 * @param[in] input
+	 * A read-only pointer to the input data.
+	 * When the input pointer is NULL, this function returns false.
+	 *
+	 * @param[out] output
+	 * A pointer to the memory (managed by the client code)
+	 * where the output data is to be written.
+	 *
+	 * @returns
+	 * True if there is output data; false otherwise.
+	 * @note
+	 * The output memory is not guaranteed to remain the same even if
+	 * the return value is false.
+	 *
+	 * @note
+	 * Derived classes should not overload this member function.
+	 * For composite filters that combine several filters
+	 * (e.g., FilterChain and FilterTree),
+	 * it is recommended to derive from this class
+	 * and override its protected member functions.
+	 * For filters that perform actual computation,
+	 * it is recommended to derive from the BaseFilter class
+	 * and only override its BaseFilter::update member function,
+	 * since the BaseFilter class already takes care of
+	 * the other protected member functions for the internal workings.
+	 */
+	bool push(void const * const input, void * const output)
+	{
+		if (input != NULL && update(input)) {
+			copy_to_client(output);
+			return true;
+		}
+		return false;
+	}
+protected:
+	/**
+	 * Read-only access to the internal output memory.
+	 *
+	 * This member function is mainly used by derived composite filters,
+	 * which needs to point the output of the previous filter stage
+	 * to the input of the next fitler stage.
+	 * See for example the implementation of FilterChain.
+	 *
+	 * @returns
+	 * A read-only pointer to the memory where the output value is stored internally
+	 * by the filter.
+	 */
+	virtual void const * const get_output_val_ptr() = 0;
+	/**
+	 * Internally update the filter output based on the given input.
+	 * This method behaves similarly to the public Filter::push method,
+	 * but without copying the output to the client memory.
+	 * This method is for internal workings of the filter framework.
+	 */
+	virtual bool update(void const * const input) = 0;
+	/**
+	 * Copy the output to client memory.
+	 */
+	virtual void copy_to_client(void * const output) = 0;
+};
+
+/**
+ * The typed filter base class.
+ *
+ * @tparam IN_T type of input data
+ * @tparam OUT_T type of output data
+ *
+ * @see Filter
+ */
+template <typename IN_T, typename OUT_T>
+class BaseFilter : public Filter
+{
+public:
+
+//// OBSOLETE NOTE ////////////////////////////////////////////////////////////
+//// The typed `push` method was originally designed to be beginner-friendly
+//// by passing by references to avoid the need to familiarize with pointers.
+//// Due to requirement of some filter classes (composite filters such as
+//// FilterChain, FilterTree, and PassThroughFilter), the input and output
+//// need to be passed by pointers.  To provide a unified use case, I have
+//// decided to remove the typed `push` method.
+////
+//// Haimo Zhang, 9 Jun 2019
+////
+//
+//	/*
+//	 * Push a new data through the filter.
+//	 * This function is essentially a proxy call to the Filter::push function
+//	 * that does away the pointer parameter, which is supposed to be
+//	 * beginner-friendly.
+//	 *
+//	 * @param[in] input A read-only reference to the input data.
+//	 * @param[out] output The reference to the output data to be written to.
+//	 *
+//	 * @returns True if there is output data; false otherwise.
+//	 * @note The output variable is not guaranteed to remain the same even if
+//	 * the return value is false.
+//	 */
+//	bool push(IN_T const &input, OUT_T &output)
+//	{
+//		return Filter::push(&input, &output);
+//	}
+///////////////////////////////////////////////////////////////////////////////
+
+protected:
+	virtual void const * const get_output_val_ptr() final { return &out_val; }
+	virtual void copy_to_client(void * const output) final
+	{
+		if (output != NULL) {
+			*(OUT_T * const) output = out_val;
+		}
+	}
+	/**
+	 * Internally managed storage of the output value.
+	 */
+	OUT_T out_val;
+};
+
+/**
+ * A pass-through filter does nothing and is useful for derived classes
+ * to perform monitoring functionalities, such as the FlowRateFilter.
+ *
+ * @tparam T The type of the data that is passing through.
+ */
+template <typename T>
+class PassThroughFilter : public Filter
+{
+public:
+	PassThroughFilter() : ptr(NULL) { }
+protected:
+	virtual void const * const get_output_val_ptr() override { return ptr; }
+	virtual bool update(void const * const input) override
+	{
+		ptr = (T const * const) input;
+		return true;
+	}
+	virtual void copy_to_client(void * const output) override
+	{
+		if (output != NULL) {
+			*(T * const) output = *ptr;
+		}
+	}
+	T const *ptr;  ///< Pointer to the latest data that passed through.
+};
+
+/**
+ * A chain of filters.
+ */
+class FilterChain : public FilterList<Filter *>, public Filter
+{
+protected:
+	virtual void const * const get_output_val_ptr() final
+	{
+		return (*last())->get_output_val_ptr();
+	}
+	virtual bool update(void const * const input) final
+	{
+		for (it = begin(); it != last(); ++it) {
+			if (!(*it)->update(it != begin() ? (*prev)->get_output_val_ptr() : input)) {
+				return false;
+			}
+			prev = it;
+		}
+		return (*it)->update((*prev)->get_output_val_ptr());
+	}
+	virtual void copy_to_client(void * const output) final
+	{
+		if (output != NULL && !isEmpty()) {
+			(*last())->copy_to_client(output);
+		}
+	}
+private:
+	FilterList<Filter *>::iterator it;
+	FilterList<Filter *>::iterator prev;
+};
+
+/**
+ * A tree of interconnected filters.
+ * While a filter has only one input, it can output to multiple other filters.
+ * Therefore, we can construct a tree of filters to simplify the interaction
+ * with complex filter graph in the client code.
+ *
+ * @todo Implement the filter tree when this use case is needed.
+ */
+class FilterTree : public Tree<Filter *>
+{
+};
+
+#endif

components/nimble_distance/nimble_distance_sensor.cpp

@@ -0,0 +1,83 @@
+#include "nimble_distance_sensor.h"
+
+namespace esphome
+{
+    namespace nimble_distance
+    {
+        static const char *const TAG = "nimble_distance";
+
+        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 NimbleDistanceSensor::get_1m_rssi(nimble_tracker::NimbleTrackerEvent *tracker_event)
+        {
+            return this->ref_rssi_ + tracker_event->getTXPower();
+        }
+
+        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 NimbleDistanceSensor::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 NimbleDistanceSensor::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);
+            return true;
+        }
+    } // namespace nimble_distance
+
+} // namespace esphome

components/nimble_distance/nimble_distance_sensor.h

@@ -0,0 +1,59 @@
+#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/components/nimble_tracker/nimble_tracker.h"
+
+namespace esphome
+{
+    namespace nimble_distance
+    {
+        class Filter
+        {
+        public:
+            Filter(float fcmin, float beta, float dcutoff);
+            bool filter(float rssi);
+            Reading<Differential<float>> output;
+
+        protected:
+            OneEuroFilter<float, unsigned long> one_euro_;
+            DifferentialFilter<float, unsigned long> diff_filter_;
+        };
+
+        class NimbleDistanceSensor : public sensor::Sensor,
+                                     public Component,
+                                     public nimble_tracker::NimbleDeviceListener
+        {
+        public:
+            void setup() override;
+            int get_1m_rssi(nimble_tracker::NimbleTrackerEvent *tracker_event);
+
+        protected:
+            bool update_state(nimble_tracker::NimbleTrackerEvent *tracker_event) override;
+            Filter *filter_;
+            int rssi_ = NO_RSSI, newest_ = NO_RSSI, recent_ = NO_RSSI, oldest_ = NO_RSSI;
+
+            int8_t ref_rssi_ = -65;
+            float absorption_ = 3.5f;
+            float last_reported_position_ = 0;
+            int64_t last_reported_micro_seconds_ = 0;
+        };
+    } // namespace nimble_distance
+} // namespace esphome

components/nimble_distance/sensor.py

@@ -0,0 +1,38 @@
+import esphome.codegen as cg
+import esphome.config_validation as cv
+from esphome.components import sensor, nimble_tracker
+from esphome.const import (
+    DEVICE_CLASS_DISTANCE,
+    STATE_CLASS_MEASUREMENT,
+    UNIT_METER,
+)
+
+DEPENDENCIES = ["nimble_tracker"]
+
+nimble_distance_ns = cg.esphome_ns.namespace("nimble_distance")
+NimbleDistanceSensor = nimble_distance_ns.class_(
+    "NimbleDistanceSensor",
+    sensor.Sensor,
+    cg.Component,
+    nimble_tracker.NimbleDeviceListener,
+)
+
+
+CONFIG_SCHEMA = cv.All(
+    sensor.sensor_schema(
+        NimbleDistanceSensor,
+        unit_of_measurement=UNIT_METER,
+        accuracy_decimals=2,
+        device_class=DEVICE_CLASS_DISTANCE,
+        state_class=STATE_CLASS_MEASUREMENT,
+    )
+    .extend(nimble_tracker.NIMBLE_DEVICE_LISTENER_SCHEMA)
+    .extend(cv.COMPONENT_SCHEMA),
+)
+
+
+async def to_code(config):
+    var = await sensor.new_sensor(config)
+    await cg.register_component(var, config)
+    await nimble_tracker.device_listener_to_code(var, config)
+    await nimble_tracker.register_ble_device(var, config)

components/nimble_distance/types.h

@@ -0,0 +1,38 @@
+#ifndef TYPES_H
+#define TYPES_H
+
+/**
+ * A class that contains a <value, timestamp> tuple.
+ *
+ * @tparam VAL_T the value type
+ * @tparam TS_T the timestamp type, defaults to `unsigned long`
+ * as per Arduino references for `millis` and `micros`.
+ */
+template <typename VAL_T, typename TS_T=unsigned long>
+class Reading
+{
+public:
+	VAL_T value;
+	TS_T timestamp;
+	Reading() : value(0), timestamp(0) {}
+	Reading(VAL_T v, TS_T ts) : value(v), timestamp(ts) { }
+};
+
+/**
+ * A class to represent the speed and acceleration of a value
+ * in addition to itself.
+ *
+ * @tparam T the type of the position, speed, and acceleration values
+ */
+template <typename T>
+class Differential
+{
+public:
+	T position;
+	T speed;
+	T acceleration;
+	Differential(T pos, T spd, T acc) : position(pos), speed(spd), acceleration(acc) { }
+	Differential(T val) : Differential(val, val, val) { }
+};
+
+#endif

components/nimble_rssi/nimble_rssi_sensor.cpp

@@ -0,0 +1,16 @@
+#include "nimble_rssi_sensor.h"
+
+namespace esphome
+{
+    namespace nimble_rssi
+    {
+        static const char *const TAG = "nimble_rssi";
+
+        bool NimbleRssiSensor::update_state(nimble_tracker::NimbleTrackerEvent *tracker_event)
+        {
+            this->publish_state(tracker_event->getRSSI());
+            return true;
+        }
+    } // namespace nimble_rssi
+
+} // namespace esphome

components/nimble_rssi/nimble_rssi_sensor.h

@@ -0,0 +1,17 @@
+#pragma once
+
+#include "esphome/core/component.h"
+#include "esphome/components/sensor/sensor.h"
+#include "esphome/components/nimble_tracker/nimble_tracker.h"
+
+namespace esphome
+{
+    namespace nimble_rssi
+    {
+        class NimbleRssiSensor : public sensor::Sensor, public Component, public nimble_tracker::NimbleDeviceListener
+        {
+        protected:
+            bool update_state(nimble_tracker::NimbleTrackerEvent *tracker_event) override;
+        };
+    } // namespace nimble_rssi
+} // namespace esphome

components/nimble_rssi/sensor.py

@@ -0,0 +1,35 @@
+import esphome.codegen as cg
+import esphome.config_validation as cv
+from esphome.components import sensor, nimble_tracker
+from esphome.const import (
+    DEVICE_CLASS_SIGNAL_STRENGTH,
+    STATE_CLASS_MEASUREMENT,
+    UNIT_DECIBEL_MILLIWATT,
+)
+
+DEPENDENCIES = ["nimble_tracker"]
+
+nimble_rssi_ns = cg.esphome_ns.namespace("nimble_rssi")
+NimbleRssiSensor = nimble_rssi_ns.class_(
+    "NimbleRssiSensor", sensor.Sensor, cg.Component, nimble_tracker.NimbleDeviceListener
+)
+
+
+CONFIG_SCHEMA = cv.All(
+    sensor.sensor_schema(
+        NimbleRssiSensor,
+        unit_of_measurement=UNIT_DECIBEL_MILLIWATT,
+        accuracy_decimals=0,
+        device_class=DEVICE_CLASS_SIGNAL_STRENGTH,
+        state_class=STATE_CLASS_MEASUREMENT,
+    )
+    .extend(nimble_tracker.NIMBLE_DEVICE_LISTENER_SCHEMA)
+    .extend(cv.COMPONENT_SCHEMA),
+)
+
+
+async def to_code(config):
+    var = await sensor.new_sensor(config)
+    await cg.register_component(var, config)
+    await nimble_tracker.device_listener_to_code(var, config)
+    await nimble_tracker.register_ble_device(var, config)

components/nimble_tracker/__init__.py

@@ -0,0 +1,110 @@
+import esphome.codegen as cg
+import esphome.config_validation as cv
+from esphome.components.esp32 import add_idf_sdkconfig_option
+from esphome.const import (
+    CONF_ACTIVE,
+    CONF_ID,
+    CONF_INTERVAL,
+    CONF_DURATION,
+)
+
+DEPENDENCIES = ["esp32"]
+
+CONF_NIMBLE_ID = "nimble_ble_id"
+CONF_SCAN_PARAMETERS = "scan_parameters"
+
+CONF_WINDOW = "window"
+CONF_CONTINUOUS = "continuous"
+
+nimble_tracker_ns = cg.esphome_ns.namespace("nimble_tracker")
+
+NimbleTracker = nimble_tracker_ns.class_("NimbleTracker", cg.Component)
+NimbleDeviceListener = nimble_tracker_ns.class_("NimbleDeviceListener", cg.Component)
+
+
+def validate_scan_parameters(config):
+    duration = config[CONF_DURATION]
+    interval = config[CONF_INTERVAL]
+    window = config[CONF_WINDOW]
+
+    if window > interval:
+        raise cv.Invalid(
+            f"Scan window ({window}) needs to be smaller than scan interval ({interval})"
+        )
+
+    if interval.total_milliseconds * 3 > duration.total_milliseconds:
+        raise cv.Invalid(
+            "Scan duration needs to be at least three times the scan interval to"
+            "cover all BLE channels."
+        )
+
+    return config
+
+
+CONFIG_SCHEMA = cv.Schema(
+    {
+        cv.GenerateID(): cv.declare_id(NimbleTracker),
+        cv.Optional(CONF_SCAN_PARAMETERS, default={}): cv.All(
+            cv.Schema(
+                {
+                    cv.Optional(
+                        CONF_DURATION, default="5min"
+                    ): cv.positive_time_period_seconds,
+                    cv.Optional(
+                        CONF_INTERVAL, default="320ms"
+                    ): cv.positive_time_period_milliseconds,
+                    cv.Optional(
+                        CONF_WINDOW, default="30ms"
+                    ): cv.positive_time_period_milliseconds,
+                    cv.Optional(CONF_ACTIVE, default=True): cv.boolean,
+                    cv.Optional(CONF_CONTINUOUS, default=True): cv.boolean,
+                    cv.Optional(CONF_CONTINUOUS, default=True): cv.boolean,
+                }
+            ),
+            validate_scan_parameters,
+        ),
+    }
+).extend(cv.COMPONENT_SCHEMA)
+
+CONF_IRK = "irk"
+
+NIMBLE_DEVICE_LISTENER_SCHEMA = cv.Schema(
+    {
+        cv.GenerateID(CONF_NIMBLE_ID): cv.use_id(NimbleTracker),
+        cv.Optional(CONF_IRK): cv.string,
+    },
+    cv.has_exactly_one_key(CONF_IRK),
+)
+
+
+async def to_code(config):
+    # this initializes the component in the generated code
+    var = cg.new_Pvariable(config[CONF_ID])
+    await cg.register_component(var, config)
+
+    params = config[CONF_SCAN_PARAMETERS]
+    cg.add(var.set_scan_duration(params[CONF_DURATION]))
+    cg.add(var.set_scan_interval(int(params[CONF_INTERVAL].total_milliseconds / 0.625)))
+    cg.add(var.set_scan_window(int(params[CONF_WINDOW].total_milliseconds / 0.625)))
+    cg.add(var.set_scan_active(params[CONF_ACTIVE]))
+    cg.add(var.set_scan_continuous(params[CONF_CONTINUOUS]))
+
+    add_idf_sdkconfig_option("CONFIG_BT_ENABLED", True)
+    add_idf_sdkconfig_option("CONFIG_BT_BLUEDROID_ENABLED", False)
+    add_idf_sdkconfig_option("CONFIG_BT_NIMBLE_ENABLED", True)
+    add_idf_sdkconfig_option("CONFIG_MBEDTLS_HARDWARE_AES", False)
+
+    cg.add_library(
+        "esp-nimble-cpp=https://github.com/h2zero/esp-nimble-cpp.git#v1.4.1", None
+    )
+
+
+async def register_ble_device(var, config):
+    paren = await cg.get_variable(config[CONF_NIMBLE_ID])
+    cg.add(paren.register_listener(var))
+    return var
+
+
+async def device_listener_to_code(var, config):
+    if CONF_IRK in config:
+        cg.add(var.set_irk(config[CONF_IRK]))

components/nimble_tracker/irk_utils.cpp

@@ -0,0 +1,61 @@
+// Copied from https://github.com/ESPresense/ESPresense/blob/master/lib/BleFingerprint/BleFingerprint.cpp
+#include "irk_utils.h"
+
+namespace esphome
+{
+    namespace nimble_tracker
+    {
+        int bt_encrypt_be(const uint8_t *key, const uint8_t *plaintext, uint8_t *enc_data)
+        {
+            mbedtls_aes_context s = {0};
+            mbedtls_aes_init(&s);
+
+            if (mbedtls_aes_setkey_enc(&s, key, 128) != 0)
+            {
+                mbedtls_aes_free(&s);
+                return BLE_HS_EUNKNOWN;
+            }
+
+            if (mbedtls_aes_crypt_ecb(&s, MBEDTLS_AES_ENCRYPT, plaintext, enc_data) != 0)
+            {
+                mbedtls_aes_free(&s);
+                return BLE_HS_EUNKNOWN;
+            }
+
+            mbedtls_aes_free(&s);
+            return 0;
+        }
+
+        bool ble_ll_resolv_rpa(const uint8_t *rpa, const uint8_t *irk)
+        {
+            struct encryption_block ecb;
+
+            auto irk32 = (const uint32_t *)irk;
+            auto key32 = (uint32_t *)&ecb.key[0];
+            auto pt32 = (uint32_t *)&ecb.plain_text[0];
+
+            key32[0] = irk32[0];
+            key32[1] = irk32[1];
+            key32[2] = irk32[2];
+            key32[3] = irk32[3];
+
+            pt32[0] = 0;
+            pt32[1] = 0;
+            pt32[2] = 0;
+            pt32[3] = 0;
+
+            ecb.plain_text[15] = rpa[3];
+            ecb.plain_text[14] = rpa[4];
+            ecb.plain_text[13] = rpa[5];
+
+            auto err = bt_encrypt_be(ecb.key, ecb.plain_text, ecb.cipher_text);
+
+            if (ecb.cipher_text[15] != rpa[0] || ecb.cipher_text[14] != rpa[1] || ecb.cipher_text[13] != rpa[2])
+                return false;
+
+            return true;
+        }
+
+    } // namespace nimble_tracker
+
+} // namespace esphome

components/nimble_tracker/irk_utils.h

@@ -0,0 +1,24 @@
+// Copied from https://github.com/ESPresense/ESPresense/blob/master/lib/BleFingerprint/BleFingerprint.cpp
+#pragma once
+
+#include "mbedtls/aes.h"
+#include "NimBLEDevice.h"
+
+namespace esphome
+{
+    namespace nimble_tracker
+    {
+        int bt_encrypt_be(const uint8_t *key, const uint8_t *plaintext, uint8_t *enc_data);
+
+        struct encryption_block
+        {
+            uint8_t key[16];
+            uint8_t plain_text[16];
+            uint8_t cipher_text[16];
+        };
+
+        bool ble_ll_resolv_rpa(const uint8_t *rpa, const uint8_t *irk);
+
+    } // namespace nimble_tracker
+
+} // namespace esphome

components/nimble_tracker/nimble_device_listener.cpp

@@ -0,0 +1,44 @@
+#include "esphome/core/log.h"
+#include "nimble_device_listener.h"
+
+namespace esphome
+{
+    namespace nimble_tracker
+    {
+        static const char *const TAG = "nimble_device_listener";
+
+        void NimbleDeviceListener::set_irk(std::string irk_hex)
+        {
+            this->match_by_ = MATCH_BY_IRK;
+            this->irk_ = new uint8_t[16];
+
+            if (!hextostr(irk_hex.c_str(), this->irk_, 16))
+            {
+                // TODO: this logic should be moved to Python validation
+                ESP_LOGE(TAG, "Something is wrong with the irk!");
+            }
+        }
+
+        bool NimbleDeviceListener::parse_event(NimbleTrackerEvent *tracker_event)
+        {
+            if (tracker_event->getAddressType() != BLE_ADDR_RANDOM)
+            {
+                return false;
+            }
+
+            auto address = tracker_event->getAddress();
+            auto naddress = address.getNative();
+
+            if (ble_ll_resolv_rpa(naddress, this->irk_))
+            {
+                ESP_LOGD(TAG, "Found device %s", tracker_event->toString().c_str());
+                return this->update_state(tracker_event);
+            }
+            else
+            {
+                return false;
+            }
+        };
+    } // namespace nimble_tracker
+
+} // namespace esphome

components/nimble_tracker/nimble_device_listener.h

@@ -0,0 +1,32 @@
+#pragma once
+
+#include "string_utils.h"
+#include "irk_utils.h"
+#include "nimble_tracker_event.h"
+
+namespace esphome
+{
+    namespace nimble_tracker
+    {
+        class NimbleDeviceListener
+        {
+
+        public:
+            bool parse_event(NimbleTrackerEvent *tracker_event);
+            void set_irk(std::string irk_hex);
+
+        protected:
+            virtual bool update_state(NimbleTrackerEvent *tracker_event) = 0;
+
+            enum MatchType
+            {
+                MATCH_BY_IRK,
+            };
+            MatchType match_by_;
+
+            uint8_t *irk_;
+        };
+
+    } // namespace nimble_tracker
+
+} // namespace esphome

components/nimble_tracker/nimble_tracker.cpp

@@ -0,0 +1,93 @@
+#include "nimble_tracker.h"
+#include "esphome/core/log.h"
+#include "esphome/core/hal.h"
+
+// using namespace esphome;
+namespace esphome
+{
+    namespace nimble_tracker
+    {
+        static const char *const TAG = "nimble_tracker";
+
+        class MyAdvertisedDeviceCallbacks : public BLEAdvertisedDeviceCallbacks
+        {
+        public:
+            MyAdvertisedDeviceCallbacks(NimbleTracker *nimble_tracker)
+            {
+                nimble_tracker_ = nimble_tracker;
+            }
+
+            void onResult(BLEAdvertisedDevice *advertised_device)
+            {
+                // Because setMaxResults is set to 0 for the NimBLEScan, we need to make a copy
+                // of the data of the advertised device, because this is deleted immediately by NimBLESCan
+                // after this callback is called.
+                auto *tracker_event = new NimbleTrackerEvent(
+                    advertised_device->getAddress(),
+                    advertised_device->getAddressType(),
+                    advertised_device->getRSSI(),
+                    advertised_device->getTXPower());
+
+                nimble_tracker_->tracker_events_.push(tracker_event);
+            }
+
+        protected:
+            NimbleTracker *nimble_tracker_;
+        };
+
+        void NimbleTracker::setup()
+        {
+            // Set the name to empty string to not broadcast the name
+            NimBLEDevice::init("");
+            this->pBLEScan_ = NimBLEDevice::getScan();
+            this->pBLEScan_->setInterval(this->scan_interval_);
+            this->pBLEScan_->setWindow(this->scan_window_);
+            this->pBLEScan_->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks(this), true);
+            this->pBLEScan_->setActiveScan(this->scan_active_);
+            this->pBLEScan_->setDuplicateFilter(false);
+            this->pBLEScan_->setMaxResults(0);
+
+            ESP_LOGV(TAG, "Trying to start the scan");
+
+            if (!pBLEScan_->start(0, nullptr, false))
+            {
+                ESP_LOGE(TAG, "Error starting continuous ble scan");
+                // this->mark_failed();
+                return;
+            }
+
+            // TODO: It takes some time to setup bluetooth? Why not just move this to loop?
+            delay(200);
+        }
+
+        void NimbleTracker::loop()
+        {
+            if (!this->pBLEScan_->isScanning())
+            {
+                if (!this->pBLEScan_->start(0, nullptr, false))
+                {
+                    ESP_LOGE(TAG, "Error starting continuous ble scan");
+                    return;
+                }
+
+                // TODO: we shouldn't block the main thread here, instead work with a setTimeout callback?
+                delay(200);
+            }
+
+            NimbleTrackerEvent *tracker_event = this->tracker_events_.pop();
+
+            while (tracker_event != nullptr)
+            {
+                for (NimbleDeviceListener *listener : this->listeners_)
+                {
+                    listener->parse_event(tracker_event);
+                }
+
+                delete tracker_event;
+                tracker_event = this->tracker_events_.pop();
+            }
+        };
+
+    } // namespace esp32_ble_tracker
+
+} // namespace esphome

components/nimble_tracker/nimble_tracker.h

@@ -0,0 +1,41 @@
+#pragma once
+
+#include "esphome/core/component.h"
+#include "queue.h"
+#include "NimBLEDevice.h"
+#include "NimBLEAdvertisedDevice.h"
+#include "nimble_device_listener.h"
+#include "nimble_tracker_event.h"
+
+namespace esphome
+{
+    namespace nimble_tracker
+    {
+        class NimbleTracker : public Component
+        {
+
+        public:
+            void set_scan_duration(uint32_t scan_duration) { scan_duration_ = scan_duration; }
+            void set_scan_interval(uint32_t scan_interval) { scan_interval_ = scan_interval; }
+            void set_scan_window(uint32_t scan_window) { scan_window_ = scan_window; }
+            void set_scan_active(bool scan_active) { scan_active_ = scan_active; }
+            void set_scan_continuous(bool scan_continuous) { scan_continuous_ = scan_continuous; }
+            Queue<NimbleTrackerEvent> tracker_events_;
+
+            void setup() override;
+            void loop() override;
+
+            void register_listener(NimbleDeviceListener *listener) { listeners_.push_back(listener); }
+
+        protected:
+            uint32_t scan_duration_;
+            uint32_t scan_interval_;
+            uint32_t scan_window_;
+            bool scan_active_;
+            bool scan_continuous_;
+            NimBLEScan *pBLEScan_;
+            std::vector<NimbleDeviceListener *> listeners_;
+        };
+    } // namespace esp32_ble_tracker
+
+} // namespace esphome

components/nimble_tracker/nimble_tracker_event.cpp

@@ -0,0 +1,45 @@
+#include "nimble_tracker_event.h"
+
+namespace esphome
+{
+    namespace nimble_tracker
+    {
+        NimbleTrackerEvent::NimbleTrackerEvent(NimBLEAddress address, uint8_t address_type, int rssi, int8_t tx_power)
+        {
+            this->address_ = address;
+            this->address_type_ = address_type;
+            this->rssi_ = rssi;
+            this->tx_power_ = tx_power;
+        }
+
+        int8_t NimbleTrackerEvent::getTXPower()
+        {
+            return this->tx_power_;
+        }
+
+        int NimbleTrackerEvent::getRSSI()
+        {
+            return this->rssi_;
+        }
+
+        uint8_t NimbleTrackerEvent::getAddressType()
+        {
+            return this->address_type_;
+        }
+
+        NimBLEAddress NimbleTrackerEvent::getAddress()
+        {
+            return this->address_;
+        }
+
+        std::string NimbleTrackerEvent::toString()
+        {
+            std::string result = "Address: " + this->address_.toString();
+            result += " Address type: " + std::to_string(this->address_type_);
+            result += " RSSI: " + std::to_string(this->rssi_);
+            result += " TX Power: " + std::to_string(this->tx_power_);
+            return result;
+        }
+    } // namespace nimble_tracker
+
+} // namespace esphome

components/nimble_tracker/nimble_tracker_event.h

@@ -0,0 +1,28 @@
+#pragma once
+
+#include "NimBLEDevice.h"
+
+namespace esphome
+{
+    namespace nimble_tracker
+    {
+        class NimbleTrackerEvent
+        {
+        public:
+            NimbleTrackerEvent(NimBLEAddress address, uint8_t address_type, int rssi, int8_t tx_power);
+            int8_t getTXPower();
+            int getRSSI();
+            uint8_t getAddressType();
+            NimBLEAddress getAddress();
+            std::string toString();
+
+        protected:
+            int8_t tx_power_;
+            int rssi_;
+            uint8_t address_type_;
+            NimBLEAddress address_;
+        };
+
+    } // namespace nimble_tracker
+
+} // namespace esphome

components/nimble_tracker/queue.h

@@ -0,0 +1,65 @@
+// Copied from https://github.com/esphome/esphome/blob/dev/esphome/components/esp32_ble_tracker/queue.h
+#pragma once
+
+#include "esphome/core/component.h"
+#include "esphome/core/helpers.h"
+
+#include <queue>
+#include <mutex>
+#include <cstring>
+
+#include <freertos/FreeRTOS.h>
+#include <freertos/semphr.h>
+
+/*
+ * BLE events come in from a separate Task (thread) in the ESP32 stack. Rather
+ * than trying to deal with various locking strategies, all incoming GAP and GATT
+ * events will simply be placed on a semaphore guarded queue. The next time the
+ * component runs loop(), these events are popped off the queue and handed at
+ * this safer time.
+ */
+
+namespace esphome
+{
+  namespace nimble_tracker
+  {
+
+    template <class T>
+    class Queue
+    {
+    public:
+      Queue() { m_ = xSemaphoreCreateMutex(); }
+
+      void push(T *element)
+      {
+        if (element == nullptr)
+          return;
+        if (xSemaphoreTake(m_, 5L / portTICK_PERIOD_MS))
+        {
+          q_.push(element);
+          xSemaphoreGive(m_);
+        }
+      }
+
+      T *pop()
+      {
+        T *element = nullptr;
+
+        if (xSemaphoreTake(m_, 5L / portTICK_PERIOD_MS))
+        {
+          if (!q_.empty())
+          {
+            element = q_.front();
+            q_.pop();
+          }
+          xSemaphoreGive(m_);
+        }
+        return element;
+      }
+
+    protected:
+      std::queue<T *> q_;
+      SemaphoreHandle_t m_;
+    };
+  } // namespace nimble_tracker
+} // namespace esphome

components/nimble_tracker/string_utils.cpp

@@ -0,0 +1,47 @@
+// Copied from https://github.com/ESPresense/ESPresense/blob/master/lib/BleFingerprint/string_utils.cpp
+#include "string_utils.h"
+
+namespace esphome
+{
+    namespace nimble_tracker
+    {
+
+        static constexpr char hexmap[] = {'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'a', 'b', 'c', 'd', 'e', 'f'};
+
+        std::string hexStr(const uint8_t *data, int len)
+        {
+            std::string s(len * 2, ' ');
+            for (int i = 0; i < len; ++i)
+            {
+                s[2 * i] = hexmap[(data[i] & 0xF0) >> 4];
+                s[2 * i + 1] = hexmap[data[i] & 0x0F];
+            }
+            return s;
+        }
+
+        uint8_t hextob(char ch)
+        {
+            if (ch >= '0' && ch <= '9')
+                return ch - '0';
+            if (ch >= 'A' && ch <= 'F')
+                return ch - 'A' + 10;
+            if (ch >= 'a' && ch <= 'f')
+                return ch - 'a' + 10;
+            return 0;
+        }
+
+        bool hextostr(const std::string &hexStr, uint8_t *output, size_t len)
+        {
+            if (len & 1)
+                return false;
+            if (hexStr.length() < len * 2)
+                return false;
+            int k = 0;
+            for (size_t i = 0; i < len * 2; i += 2)
+                output[k++] = (hextob(hexStr[i]) << 4) | hextob(hexStr[i + 1]);
+            return true;
+        }
+
+    } // namespace nimble_tracker
+
+} // namespece esphome

components/nimble_tracker/string_utils.h

@@ -0,0 +1,18 @@
+// Copied from https://github.com/ESPresense/ESPresense/blob/master/lib/BleFingerprint/string_utils.h
+#pragma once
+#include <string>
+
+#define Sprintf(f, ...) ({ char* s; asprintf(&s, f, __VA_ARGS__); std::string r = s; free(s); r; })
+
+namespace esphome
+{
+    namespace nimble_tracker
+    {
+
+        std::string hexStr(const uint8_t *data, int len);
+        uint8_t hextob(char ch);
+        bool hextostr(const std::string &hexStr, uint8_t *output, size_t len);
+
+    } // namespace nimble_tracker
+
+} // namespace esphome

config.yaml

@@ -0,0 +1,110 @@
+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"