wip

components/nimble_distance_custom/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_custom/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_custom/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_custom/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_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/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_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<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);
+        }
+
+        // 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

components/nimble_distance_custom/nimble_distance_custom.h

@@ -0,0 +1,65 @@
+#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/nimble_tracker/nimble_tracker.h"
+
+namespace esphome
+{
+    namespace nimble_distance_custom
+    {
+        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_;
+        };
+
+        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);
+
+        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_ = -75;
+            float absorption_ = 3.5f;
+            float last_reported_position_ = 0;
+            int64_t last_reported_micro_seconds_ = 0;
+        };
+    } // namespace nimble_distance
+} // namespace esphome

components/nimble_distance_custom/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