JacksoTheGreat/follow-me-stairs
1
0 Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Follow-me stair light for the Apollo MTR-1 and WLED

Browse Source 
ESPHome package for the Apollo MTR-1: tracks people on the stairs with the
LD2450 radar and streams a lit window to a WLED controller over DDP. The
sensor serves a setup wizard that measures the stairs with one walk and
stores the setup on the device.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
This commit is contained in:
Jackson AndresandClaude Opus 5.5 committed 2026-10-14 19:31:00 -04:00
commit 6831bbf9a8
24 files changed
+10874
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

firmware/follow-me-button.js
+29
View File
@@ -0,0 +1,29 @@
// Adds a "Set up Follow-Me" link to ESPHome's stock web page, opening the
// setup wizard the device serves at /followme. Loaded by the stock page as
// /0.js (web_server js_include in follow-me.yaml).
(() => {
const ID = 'followme-setup-link';
const add = () => {
if (document.getElementById(ID)) return;
const a = document.createElement('a');
a.id = ID;
a.href = '/followme';
a.textContent = 'Set up Follow-Me';
a.style.cssText = [
'position:fixed',
'right:20px',
'bottom:20px',
'z-index:9999',
'padding:10px 16px',
'background:#0e0e10',
'color:#ece8df',
'border:1px solid rgba(236,232,223,.35)',
'border-radius:3px',
"font:500 14px/1.2 'IBM Plex Sans',system-ui,sans-serif",
'text-decoration:none',
].join(';');
document.body.appendChild(a);
};
if (document.readyState === 'loading') document.addEventListener('DOMContentLoaded', add);
else add();
})();
firmware/follow-me-calibration.h
+630
View File
@@ -0,0 +1,630 @@
#pragma once
// The stairs calibration made by the setup wizard (wizard/wizard.html), stored
// on the device, and the geometry derived from it.
//
// Storage: one flash preference holding the calibration and the wizard's own
// state (an opaque JSON string the wizard reads back to resume). It survives
// reboots and firmware updates.
//
// HTTP, on the device's web server:
// GET /followme the setup wizard (follow-me-wizard.h, built by
// wizard/tools/embed.mjs), gzipped, fonts
// inlined; works with no internet access.
//
// GET /followme/calibration {"set":false} or {"set":true, ...calibration}
// POST /followme/calibration body: calibration JSON, sent as text/plain so
// a page on another origin needs no preflight.
// 200 {"ok":true} once validated; 400 with
// {"ok":false,"error":"..."} otherwise.
//
// GET /followme/targets {"t": ms, "targets": [[x, y] | null, ...]}: the
// radar's three targets now, raw (mm, x as
// reported), unthrottled. The wizard polls it
// for its live view.
// POST /followme/preview body {"wled": host, "led": n} as text/plain.
// Streams LED n (1-based, raw strip index, as
// follow-me addresses it) lit and the rest
// dark, to that WLED for
// PREVIEW_MS after the last request. led 0 ends
// it. Works with or without a calibration; the
// setup wizard uses it to match LEDs to spots.
//
// Calibration JSON (lengths in mm, in the LD2450's own frame, x as reported):
// cl 2 to 4 points [x, y]: the walking path from the lower floor to the
// upper floor (2 = straight, 3 = L, 4 = U).
// bottom, top distance along cl of the front of the bottom step
// and the top edge of the top step.
// landIn, landOut distance along cl of the landing edges (L, U only).
// steps treads per flight, [n] or [lower, upper].
// width stair width.
// wled WLED controller address (IPv4 or host name). The LED count and
// whether the strip has a white channel are read from WLED itself.
// anchors 2 to 8 pairs [d, led]: LED number (1-based) whose centre sits at
// distance d along cl. LEDs between anchors are interpolated, LEDs
// beyond the outer anchors extrapolated.
// wizard wizard state, stored and returned verbatim.
//
// Threading: the HTTP handler runs on the web server task. It validates into
// a pending copy; the main loop applies it (apply_pending()), saves it, and
// rebuilds the derived geometry. Geometry is read only from the main loop.
#include <atomic>
#include <cctype>
#include <cmath>
#include <cstdio>
#include <cstring>
#include <memory>
#include <mutex>
#include <string>
#include <utility>
#include "follow-me.h"
#include "follow-me-wizard.h"
#include "esphome/components/json/json_util.h"
#include "esphome/components/sensor/sensor.h"
#include "esphome/components/web_server_base/web_server_base.h"
#include "esphome/core/hal.h"
#include "esphome/core/helpers.h"
#include "esphome/core/log.h"
#include "esphome/core/preferences.h"
namespace follow_me {
static const char *const CAL_TAG = "follow_me.cal";
static const uint32_t CAL_VERSION = 1;
static const int CAL_MAX_POINTS = 4;
static const int CAL_MAX_ANCHORS = 8;
// Longest strip follow-me drives. LED positions are kept for every LED, so
// this sets the RAM used (4 bytes per LED) and the frame buffer (4 per LED).
static const int MAX_STREAM_LEDS = 1500;
static const int CAL_MAX_LED_NUMBER = MAX_STREAM_LEDS;
static const size_t CAL_WIZARD_MAX = 1536;
static const size_t CAL_BODY_MAX = 4096;
struct Calibration {
uint32_t version;
uint8_t points;
float x[CAL_MAX_POINTS], y[CAL_MAX_POINTS];
float bottom, top, land_in, land_out;
uint8_t steps[2];
float width;
char wled[HOST_MAX];
uint8_t anchors;
float anchor_d[CAL_MAX_ANCHORS], anchor_led[CAL_MAX_ANCHORS];
char wizard[CAL_WIZARD_MAX];
};
// Geometry used by the follow-me loop. Positions are inches along the stairs
// from the top edge of the top step, increasing down the stairs ("wall
// inches"; effects are laid out in the same units).
static const int MAX_GAPS = 8;
static const float GAP_FACTOR = 3.0f;
struct Geometry {
bool ready = false;
int points = 0;
float x[CAL_MAX_POINTS] = {}, y[CAL_MAX_POINTS] = {}; // path, inches
float cum[CAL_MAX_POINTS] = {}; // inches along the path at each point
float top_along = 0; // inches along the path of the top edge
float stairs_bottom = 0; // wall inches of the front of the bottom step
float inches_per_step = 0;
float half_width = 0;
float edge_wall[MAX_STREAM_LEDS + 1] = {}; // wall inches of LED edge k (edge 0 = outer edge of LED 1)
// Drawing coordinate: wall inches with the strip's gaps squeezed out. A gap
// is a span between two pinned LEDs with far fewer LEDs than its length
// needs (a jumper across a landing, a corner, a doorway): each LED there
// covers more than GAP_FACTOR times the strip's usual spacing. In drawing
// coordinates each such LED covers the usual spacing, so a window crossing
// the gap lights both sides of it.
int gaps = 0;
float gap_lo[MAX_GAPS] = {}, gap_hi[MAX_GAPS] = {}, gap_cut[MAX_GAPS] = {}; // wall span and inches removed
float edge_draw[MAX_STREAM_LEDS + 1] = {}; // drawing coordinate of LED edge k
};
inline Calibration active_cal{};
inline Calibration pending_cal{};
inline bool has_active = false;
inline std::atomic<bool> has_pending{false};
inline std::mutex cal_mutex;
inline Geometry geo;
inline ESPPreferenceObject cal_pref;
inline float path_length(const Calibration &c) {
float total = 0;
for (int i = 1; i < c.points; i++) total += hypotf(c.x[i] - c.x[i - 1], c.y[i] - c.y[i - 1]);
return total;
}
// Empty string if the calibration can drive the light, otherwise why not.
inline std::string validate(const Calibration &c) {
if (c.points < 2 || c.points > CAL_MAX_POINTS) return "cl needs 2 to 4 points";
for (int i = 1; i < c.points; i++)
if (hypotf(c.x[i] - c.x[i - 1], c.y[i] - c.y[i - 1]) < 100) return "cl has points closer than 100 mm";
const float total = path_length(c);
if (!(c.bottom >= 0 && c.top > c.bottom + 300 && c.top <= total + 1)) return "bottom and top must lie on cl, top past bottom";
const int flights = c.points == 2 ? 1 : 2;
if (flights == 2 && !(c.land_in > c.bottom && c.land_out >= c.land_in && c.top > c.land_out))
return "landIn and landOut must lie between bottom and top";
for (int f = 0; f < flights; f++)
if (c.steps[f] < 1 || c.steps[f] > 40) return "steps must be 1 to 40 per flight";
if (!(c.width >= 300 && c.width <= 2500)) return "width must be 300 to 2500 mm";
if (!c.wled[0]) return "wled address is required";
if (c.anchors < 2 || c.anchors > CAL_MAX_ANCHORS) return "anchors needs 2 to 8 pairs";
for (int i = 0; i < c.anchors; i++) {
if (!(c.anchor_led[i] >= 1 && c.anchor_led[i] <= CAL_MAX_LED_NUMBER)) return "anchor LED is past the longest strip follow-me drives";
for (int j = 0; j < i; j++)
if (fabsf(c.anchor_led[i] - c.anchor_led[j]) < 0.5f) return "two anchors share an LED";
}
return "";
}
// Drawing coordinate of a wall position: the gaps' removed inches before it
// are subtracted; inside a gap the removal is spread evenly.
inline float draw_pos(float wall) {
float out = wall;
for (int i = 0; i < geo.gaps; i++) {
const float lo = geo.gap_lo[i], hi = geo.gap_hi[i], cut = geo.gap_cut[i];
if (wall >= hi) out -= cut;
else if (wall > lo) out -= cut * (wall - lo) / (hi - lo);
}
return out;
}
// Rebuilds geo from a validated calibration.
inline void derive(const Calibration &c) {
Geometry &g = geo;
g.points = c.points;
for (int i = 0; i < c.points; i++) {
g.x[i] = c.x[i] / 25.4f;
g.y[i] = c.y[i] / 25.4f;
g.cum[i] = i == 0 ? 0 : g.cum[i - 1] + hypotf(g.x[i] - g.x[i - 1], g.y[i] - g.y[i - 1]);
}
g.top_along = c.top / 25.4f;
g.stairs_bottom = (c.top - c.bottom) / 25.4f;
const float treads = c.points == 2 ? c.top - c.bottom : (c.land_in - c.bottom) + (c.top - c.land_out);
const int steps = c.steps[0] + (c.points == 2 ? 0 : c.steps[1]);
g.inches_per_step = treads / steps / 25.4f;
g.half_width = c.width / 2 / 25.4f;
// Anchors as (LED edge, wall inches), sorted by edge. LED n's centre is
// edge n - 0.5.
float ae[CAL_MAX_ANCHORS], aw[CAL_MAX_ANCHORS];
const int n = c.anchors;
for (int i = 0; i < n; i++) {
ae[i] = c.anchor_led[i] - 0.5f;
aw[i] = (c.top - c.anchor_d[i]) / 25.4f;
}
for (int i = 1; i < n; i++)
for (int j = i; j > 0 && ae[j] < ae[j - 1]; j--) {
std::swap(ae[j], ae[j - 1]);
std::swap(aw[j], aw[j - 1]);
}
// Interpolated between anchors, extrapolated past the outer ones.
for (int k = 0; k <= MAX_STREAM_LEDS; k++) {
int j = 0;
while (j < n - 2 && k > ae[j + 1]) j++;
g.edge_wall[k] = aw[j] + (k - ae[j]) * (aw[j + 1] - aw[j]) / (ae[j + 1] - ae[j]);
}
// Gaps: per-LED spacing within the pinned range, against its median.
{
int lo = MAX_STREAM_LEDS, hi = 0;
for (int i = 0; i < n; i++) {
lo = std::min(lo, (int) floorf(ae[i]));
hi = std::max(hi, (int) ceilf(ae[i]));
}
lo = std::max(lo, 0);
hi = std::min(hi, MAX_STREAM_LEDS);
static float pitch[MAX_STREAM_LEDS];
int m = 0;
for (int k = lo; k < hi; k++) pitch[m++] = fabsf(g.edge_wall[k + 1] - g.edge_wall[k]);
g.gaps = 0;
if (m > 0) {
static float sorted[MAX_STREAM_LEDS];
memcpy(sorted, pitch, m * sizeof(float));
std::nth_element(sorted, sorted + m / 2, sorted + m);
const float usual = sorted[m / 2];
for (int k = lo; k < hi && usual > 0.0f; k++) {
const float a = fminf(g.edge_wall[k], g.edge_wall[k + 1]), b = fmaxf(g.edge_wall[k], g.edge_wall[k + 1]);
if (b - a <= GAP_FACTOR * usual) continue;
if (g.gaps > 0 && fabsf(g.gap_hi[g.gaps - 1] - a) < 0.01f) { // continues the previous gap
g.gap_hi[g.gaps - 1] = b;
g.gap_cut[g.gaps - 1] += b - a - usual;
} else if (g.gaps > 0 && fabsf(g.gap_lo[g.gaps - 1] - b) < 0.01f) {
g.gap_lo[g.gaps - 1] = a;
g.gap_cut[g.gaps - 1] += b - a - usual;
} else if (g.gaps < MAX_GAPS) {
g.gap_lo[g.gaps] = a;
g.gap_hi[g.gaps] = b;
g.gap_cut[g.gaps] = b - a - usual;
g.gaps++;
}
}
}
}
g.ready = true;
for (int k = 0; k <= MAX_STREAM_LEDS; k++) g.edge_draw[k] = draw_pos(g.edge_wall[k]);
for (int i = 0; i < g.gaps; i++)
ESP_LOGI(CAL_TAG, "Strip gap at wall %.1f-%.1f in, drawn %.1f in shorter", g.gap_lo[i], g.gap_hi[i], g.gap_cut[i]);
set_wled_host(c.wled);
ESP_LOGI(CAL_TAG, "Calibration applied: %d-point path, %.1f in per step, stairs %.1f in long, WLED %s", c.points,
g.inches_per_step, g.stairs_bottom, c.wled);
}
// Nearest point on the path to (x, y) in radar inches. Returns the squared
// distance; *wall is the wall-inch position there, *at_end whether the point
// lies beyond either end of the path.
inline float project(float x, float y, float *wall, bool *at_end) {
const Geometry &g = geo;
float best = 1e9f;
for (int k = 0; k < g.points - 1; k++) {
const float sx = g.x[k + 1] - g.x[k], sy = g.y[k + 1] - g.y[k];
float f = ((x - g.x[k]) * sx + (y - g.y[k]) * sy) / (sx * sx + sy * sy);
bool end = false;
if (f < 0.0f) {
f = 0.0f;
end = k == 0;
}
if (f > 1.0f) {
f = 1.0f;
end = k == g.points - 2;
}
const float ex = x - (g.x[k] + f * sx), ey = y - (g.y[k] + f * sy);
const float d2 = ex * ex + ey * ey;
if (d2 < best) {
best = d2;
*wall = g.top_along - (g.cum[k] + f * (g.cum[k + 1] - g.cum[k]));
*at_end = end;
}
}
return best;
}
// Main loop: load the stored calibration once, then pick up new ones.
inline void apply_pending() {
if (!has_pending.load()) return;
{
std::lock_guard<std::mutex> lock(cal_mutex);
active_cal = pending_cal;
has_active = true;
has_pending = false;
}
if (cal_pref.save(&active_cal) && global_preferences->sync())
ESP_LOGI(CAL_TAG, "Calibration saved");
else
ESP_LOGW(CAL_TAG, "Calibration could not be saved to flash");
derive(active_cal);
}
inline void load_calibration() {
cal_pref = global_preferences->make_preference<Calibration>(fnv1_hash("follow_me_calibration"));
static Calibration c;
if (!cal_pref.load(&c) || c.version != CAL_VERSION) {
ESP_LOGI(CAL_TAG, "No calibration stored; run the setup wizard");
return;
}
c.wizard[CAL_WIZARD_MAX - 1] = '\0';
c.wled[HOST_MAX - 1] = '\0';
const std::string err = validate(c);
if (!err.empty()) {
ESP_LOGW(CAL_TAG, "Stored calibration rejected: %s", err.c_str());
return;
}
{
std::lock_guard<std::mutex> lock(cal_mutex);
active_cal = c;
has_active = true;
}
derive(c);
}
// ---------- JSON ----------
inline std::string to_json(const Calibration &c) {
std::string out = "{\"set\":true,\"cl\":[";
char buf[64];
for (int i = 0; i < c.points; i++) {
snprintf(buf, sizeof(buf), "%s[%.1f,%.1f]", i ? "," : "", c.x[i], c.y[i]);
out += buf;
}
snprintf(buf, sizeof(buf), "],\"bottom\":%.1f,\"top\":%.1f", c.bottom, c.top);
out += buf;
if (c.points > 2) {
snprintf(buf, sizeof(buf), ",\"landIn\":%.1f,\"landOut\":%.1f", c.land_in, c.land_out);
out += buf;
}
if (c.points > 2)
snprintf(buf, sizeof(buf), ",\"steps\":[%u,%u]", c.steps[0], c.steps[1]);
else
snprintf(buf, sizeof(buf), ",\"steps\":[%u]", c.steps[0]);
out += buf;
snprintf(buf, sizeof(buf), ",\"width\":%.1f,\"wled\":\"", c.width);
out += buf;
for (const char *p = c.wled; *p; p++)
if (*p != '"' && *p != '\\') out += *p; // validated to be a plain host; kept JSON-safe regardless
out += "\",\"anchors\":[";
for (int i = 0; i < c.anchors; i++) {
snprintf(buf, sizeof(buf), "%s[%.1f,%.2f]", i ? "," : "", c.anchor_d[i], c.anchor_led[i]);
out += buf;
}
out += "],\"wizard\":";
out += c.wizard[0] ? c.wizard : "null";
out += "}";
return out;
}
// Parses a POST body into c. Empty string on success, otherwise the error.
inline std::string from_json(const std::string &body, Calibration &c) {
c = Calibration{};
c.version = CAL_VERSION;
std::string err = "body is not a JSON object";
esphome::json::parse_json(body, [&](JsonObject o) -> bool {
JsonArray cl = o["cl"].as<JsonArray>(), steps = o["steps"].as<JsonArray>(), an = o["anchors"].as<JsonArray>();
if (cl.isNull() || steps.isNull() || an.isNull()) {
err = "cl, steps and anchors are required";
return false;
}
if (cl.size() < 2 || cl.size() > CAL_MAX_POINTS) {
err = "cl needs 2 to 4 points";
return false;
}
c.points = cl.size();
for (int i = 0; i < c.points; i++) {
c.x[i] = cl[i][0] | NAN;
c.y[i] = cl[i][1] | NAN;
}
c.bottom = o["bottom"] | NAN;
c.top = o["top"] | NAN;
c.land_in = o["landIn"] | NAN;
c.land_out = o["landOut"] | NAN;
if (steps.size() != (c.points == 2 ? 1u : 2u)) {
err = "steps needs one count per flight";
return false;
}
for (size_t i = 0; i < steps.size(); i++) c.steps[i] = steps[i] | 0;
c.width = o["width"] | NAN;
const char *wled = o["wled"] | "";
if (strlen(wled) >= HOST_MAX) {
err = "wled address is too long";
return false;
}
for (const char *p = wled; *p; p++)
if (!(isalnum((unsigned char) *p) || *p == '.' || *p == '-' || *p == '_')) {
err = "wled must be an IPv4 address or host name";
return false;
}
strcpy(c.wled, wled);
if (an.size() < 2 || an.size() > CAL_MAX_ANCHORS) {
err = "anchors needs 2 to 8 pairs";
return false;
}
c.anchors = an.size();
for (int i = 0; i < c.anchors; i++) {
c.anchor_d[i] = an[i][0] | NAN;
c.anchor_led[i] = an[i][1] | NAN;
}
JsonVariant wiz = o["wizard"];
if (!wiz.isNull()) {
std::string s;
serializeJson(wiz, s);
if (s.size() >= CAL_WIZARD_MAX) {
err = "wizard state is too large";
return false;
}
memcpy(c.wizard, s.c_str(), s.size() + 1);
}
for (int i = 0; i < c.points; i++)
if (std::isnan(c.x[i]) || std::isnan(c.y[i])) {
err = "cl points must be [x, y] numbers";
return false;
}
err = validate(c);
return err.empty();
});
return err;
}
// The radar's target X/Y sensors, found by the follow-me loop.
inline esphome::sensor::Sensor *radar_x[MAX_TRACKS] = {}, *radar_y[MAX_TRACKS] = {};
inline std::string targets_json() {
std::string out = "{\"t\":" + std::to_string(millis()) + ",\"targets\":[";
char buf[48];
for (int t = 0; t < MAX_TRACKS; t++) {
if (t) out += ",";
const float x = radar_x[t] ? radar_x[t]->get_raw_state() : NAN, y = radar_y[t] ? radar_y[t]->get_raw_state() : NAN;
if (std::isnan(x) || std::isnan(y) || (x == 0 && y == 0)) {
out += "null";
} else {
snprintf(buf, sizeof(buf), "[%.0f,%.0f]", x, y);
out += buf;
}
}
return out + "]}";
}
// ---------- LED preview ----------
static const uint32_t PREVIEW_MS = 10000;
inline char preview_host[HOST_MAX] = "";
inline int preview_led = 0; // guarded by cal_mutex
inline uint32_t preview_until = 0; // millis(); guarded by cal_mutex
// Main loop: the LED to preview now (0 = none). Points the WLED watcher at
// the preview's WLED while previewing and back at the calibration's after.
inline int preview_now(uint32_t now) {
static bool was = false;
int led = 0;
char host[HOST_MAX];
{
std::lock_guard<std::mutex> lock(cal_mutex);
if (preview_led > 0 && (int32_t) (preview_until - now) > 0) {
led = preview_led;
memcpy(host, preview_host, HOST_MAX);
} else {
preview_led = 0;
}
}
if (led > 0) {
set_wled_host(host);
was = true;
} else if (was) {
was = false;
set_wled_host(has_active ? active_cal.wled : "");
}
return led;
}
// Fills data (bpl bytes per LED) with the preview of LED led (1-based) for a
// strip of leds LEDs: that one LED bright amber, the rest dark. Returns the
// data length.
inline int preview_frame(uint8_t *data, int leds, int bpl, int led) {
const int len = leds * bpl;
memset(data, 0, len);
static const uint8_t LIT[3] = {255, 110, 0};
if (led >= 1 && led <= leds)
for (int ch = 0; ch < 3; ch++) data[(led - 1) * bpl + ch] = LIT[ch];
return len;
}
inline std::string parse_preview(const std::string &body) {
std::string err = "body is not a JSON object";
esphome::json::parse_json(body, [&](JsonObject o) -> bool {
const char *wled = o["wled"] | "";
const int led = o["led"] | -1;
if (led < 0 || led > CAL_MAX_LED_NUMBER) {
err = "led is past the longest strip follow-me drives";
return false;
}
if (led > 0) {
if (!wled[0] || strlen(wled) >= HOST_MAX) {
err = "wled address is required";
return false;
}
for (const char *p = wled; *p; p++)
if (!(isalnum((unsigned char) *p) || *p == '.' || *p == '-' || *p == '_')) {
err = "wled must be an IPv4 address or host name";
return false;
}
}
std::lock_guard<std::mutex> lock(cal_mutex);
preview_led = led;
if (led > 0) {
strcpy(preview_host, wled);
preview_until = millis() + PREVIEW_MS;
}
err = "";
return true;
});
return err;
}
// ---------- HTTP ----------
class CalibrationHandler : public AsyncWebHandler {
public:
bool canHandle(AsyncWebServerRequest *request) const override {
char buf[AsyncWebServerRequest::URL_BUF_SIZE];
const StringRef url = request->url_to(buf);
const auto m = request->method();
return ((url == "/followme" || url == "/followme/") && m == HTTP_GET) ||
(url == "/followme/calibration" && (m == HTTP_GET || m == HTTP_POST)) ||
(url == "/followme/preview" && m == HTTP_POST) || (url == "/followme/targets" && m == HTTP_GET);
}
void handleBody(AsyncWebServerRequest *request, uint8_t *data, size_t len, size_t index, size_t total) override {
if (index == 0) {
this->body_.clear();
this->too_big_ = total > CAL_BODY_MAX;
if (!this->too_big_) this->body_.reserve(total);
}
if (!this->too_big_) this->body_.append(reinterpret_cast<const char *>(data), len);
}
void handleRequest(AsyncWebServerRequest *request) override {
char ubuf[AsyncWebServerRequest::URL_BUF_SIZE];
const StringRef url = request->url_to(ubuf);
if (url == "/followme" || url == "/followme/") {
AsyncWebServerResponse *response =
request->beginResponse(200, "text/html; charset=utf-8", WIZARD_HTML_GZ, WIZARD_HTML_GZ_LEN);
response->addHeader("Content-Encoding", "gzip");
response->addHeader("Cache-Control", "no-cache");
request->send(response);
return;
}
if (url == "/followme/targets") {
const std::string out = targets_json();
request->send(200, "application/json", out.c_str());
return;
}
if (request->method() == HTTP_GET) {
std::string out;
{
std::lock_guard<std::mutex> lock(cal_mutex);
if (has_pending.load())
out = to_json(pending_cal);
else if (has_active)
out = to_json(active_cal);
else
out = "{\"set\":false}";
}
request->send(200, "application/json", out.c_str());
return;
}
if (url == "/followme/preview") {
const std::string err = this->too_big_ ? std::string("body is too large") : parse_preview(this->body_);
this->body_.clear();
if (!err.empty()) {
const std::string out = "{\"ok\":false,\"error\":\"" + err + "\"}";
request->send(400, "application/json", out.c_str());
} else {
request->send(200, "application/json", "{\"ok\":true}");
}
return;
}
// Heap, not stack: the web server task has little stack to spare.
auto c = std::make_unique<Calibration>();
std::string err;
if (this->too_big_)
err = "body is too large";
else
err = from_json(this->body_, *c);
this->body_.clear();
this->body_.shrink_to_fit();
if (!err.empty()) {
ESP_LOGW(CAL_TAG, "Calibration refused: %s", err.c_str());
const std::string out = "{\"ok\":false,\"error\":\"" + err + "\"}";
request->send(400, "application/json", out.c_str());
return;
}
{
std::lock_guard<std::mutex> lock(cal_mutex);
pending_cal = *c;
has_pending = true;
}
request->send(200, "application/json", "{\"ok\":true}");
}
bool isRequestHandlerTrivial() const override { return false; }
protected:
std::string body_;
bool too_big_ = false;
};
inline bool web_ready = false; // the device config has a web server; the wizard needs it
inline bool radar_missing = false; // set by the follow-me loop when the radar target sensors are not found
// Called once at boot, after the web server is set up.
inline void setup_calibration() {
start_watch();
load_calibration();
if (web_server_base::global_web_server_base == nullptr) {
ESP_LOGE(CAL_TAG, "No web server in this device config; the setup wizard cannot reach the device");
return;
}
web_server_base::global_web_server_base->add_handler(new CalibrationHandler()); // NOLINT
web_ready = true;
}
} // namespace follow_me
firmware/follow-me-effects.h
+389
View File
@@ -0,0 +1,389 @@
#pragma once
// Effects for the follow-me glow (follow-me.yaml).
//
// Each effect paints one LED's colour inside a lit window; the renderer then
// scales it by the Follow-Me Glow brightness and the window's coverage and
// fade, exactly as it scales the plain colour. Patterns are fixed to the
// stairs (wall inches), not to the window, so a person walks through them.
// Wall inches increase down the stairs; "falling" patterns move toward larger
// wall values, "rising" ones toward smaller.
//
// Colours are written as perceptual (sRGB-like) values 0-1 per R, G, B and
// the strip's white channel; paint() squares them (gamma 2) so
// saturated mixes such as orange stay orange on the LEDs.
//
// Nothing here depends on ESPHome, so the file also compiles on a desktop for
// previews. EFFECT_NAMES must match the effect names on the Follow-Me Glow
// light in follow-me.yaml.
#include <algorithm>
#include <cmath>
#include <cstdint>
#include <cstring>
namespace follow_me {
namespace fx {
enum Effect {
NONE = 0,
RAINBOW,
NEW_YEARS,
VALENTINES,
ST_PATRICKS,
EASTER,
FOURTH_OF_JULY,
AUTUMN,
HALLOWEEN,
HAUNTED,
HANUKKAH,
CHRISTMAS,
CANDY_CANE,
SNOWFALL,
FIREPLACE,
AURORA,
OCEAN,
EFFECT_COUNT,
};
static const char *const EFFECT_NAMES[EFFECT_COUNT] = {
"None", "Rainbow", "New Year's", "Valentine's", "St. Patrick's", "Easter",
"Fourth of July", "Autumn", "Halloween", "Haunted", "Hanukkah", "Christmas",
"Candy Cane", "Snowfall", "Fireplace", "Aurora", "Ocean",
};
// Length of the stairs in wall inches, for patterns that span them
// (Snowfall). Set by the follow-me loop every frame.
inline float extent = 240.0f;
inline void set_extent(float wall_inches) { extent = fmaxf(wall_inches, 24.0f); }
inline Effect from_name(const char *name, size_t len) {
for (int i = 1; i < EFFECT_COUNT; i++) {
if (strlen(EFFECT_NAMES[i]) == len && strncmp(EFFECT_NAMES[i], name, len) == 0) return (Effect) i;
}
return NONE;
}
struct Rgbw {
float r, g, b, w;
};
inline Rgbw scale(const Rgbw &c, float k) { return {c.r * k, c.g * k, c.b * k, c.w * k}; }
inline Rgbw mix(const Rgbw &a, const Rgbw &b, float f) {
return {a.r + (b.r - a.r) * f, a.g + (b.g - a.g) * f, a.b + (b.b - a.b) * f, a.w + (b.w - a.w) * f};
}
inline Rgbw lighten(const Rgbw &a, const Rgbw &b) { // per-channel max
return {fmaxf(a.r, b.r), fmaxf(a.g, b.g), fmaxf(a.b, b.b), fmaxf(a.w, b.w)};
}
inline float clamp01(float x) { return x < 0.0f ? 0.0f : (x > 1.0f ? 1.0f : x); }
inline float smoothstep(float x) {
x = clamp01(x);
return x * x * (3.0f - 2.0f * x);
}
inline float fract(float x) { return x - floorf(x); }
inline uint32_t hash(uint32_t x) {
x ^= x >> 16;
x *= 0x7feb352du;
x ^= x >> 15;
x *= 0x846ca68bu;
x ^= x >> 16;
return x;
}
// Deterministic random 0-1 for a (seed, n) pair.
inline float rnd(uint32_t seed, uint32_t n) { return (hash(n * 0x9e3779b1u ^ hash(seed + 0x632be5abu)) >> 8) / 16777216.0f; }
// Smooth 1D value noise, 0-1, one feature per unit of x.
inline float noise(uint32_t seed, float x) {
const float i = floorf(x);
const int32_t n = (int32_t) i;
return rnd(seed, (uint32_t) n) + (rnd(seed, (uint32_t) (n + 1)) - rnd(seed, (uint32_t) n)) * smoothstep(x - i);
}
// Smooth 2D value noise, 0-1, one feature per unit of x and of y.
inline float noise2(uint32_t seed, float x, float y) {
const float j = floorf(y);
const int32_t m = (int32_t) j;
const float a = noise(seed ^ hash((uint32_t) m), x), b = noise(seed ^ hash((uint32_t) (m + 1)), x);
return a + (b - a) * smoothstep(y - j);
}
// Soft-edged stripes: 1 for the first `duty` of each `period`, 0 for the
// rest, with linear edges `soft` units wide (anti-aliased between LEDs).
inline float stripe(float s, float period, float duty, float soft) {
const float p = fract(s / period) * period;
const float on = duty * period;
const float h = soft / 2.0f;
const float rise = clamp01((p + h) / soft); // edge at p = 0, wrapped below
const float fall = clamp01((on + h - p) / soft);
const float wrap = clamp01((p - (period - h)) / soft); // edge at p = period
return fmaxf(fminf(rise, fall), wrap);
}
// Blocks of `size` units, each a random entry of the palette, blended over the
// last `blend` fraction of each block.
inline Rgbw blocks(float s, float size, uint32_t seed, const Rgbw *pal, int n, float blend) {
const float u = s / size;
const float i = floorf(u);
const int32_t k = (int32_t) i;
const Rgbw a = pal[hash(seed ^ hash((uint32_t) k)) % n];
const Rgbw b = pal[hash(seed ^ hash((uint32_t) (k + 1))) % n];
return mix(a, b, smoothstep((u - i - (1.0f - blend)) / blend));
}
// Twinkle envelope 0-1 for one LED: in each `period` seconds the LED flashes
// with probability `chance` (fast rise, slower fall). *slot_out is the period
// index, for picking the twinkle's colour.
inline float twinkle(int led, float t, float period, float chance, uint32_t seed, uint32_t *slot_out = nullptr) {
const float u = t / period + rnd(seed, (uint32_t) led);
const uint32_t slot = (uint32_t) floorf(u);
if (slot_out) *slot_out = slot;
if (rnd(seed ^ 0x5bd1e995u, slot * 1031u + (uint32_t) led) > chance) return 0.0f;
const float f = u - floorf(u);
const float attack = 0.12f;
if (f < attack) return f / attack;
const float d = 1.0f - (f - attack) / (1.0f - attack);
return d * d;
}
inline Rgbw hsv(float h, float s, float v) {
h = fract(h) * 6.0f;
const int i = (int) h;
const float f = h - i;
const float p = v * (1.0f - s), q = v * (1.0f - s * f), t = v * (1.0f - s * (1.0f - f));
switch (i) {
case 0: return {v, t, p, 0};
case 1: return {q, v, p, 0};
case 2: return {p, v, t, 0};
case 3: return {p, q, v, 0};
case 4: return {t, p, v, 0};
default: return {v, p, q, 0};
}
}
// Firework burst: every `period` seconds a ring expands from a random point
// inside the window and fades. Returns 0-1; *slot_out picks its colour.
inline float burst(float wall, float t, float period, uint32_t seed, float focus, float span, uint32_t *slot_out) {
const float u = t / period;
const uint32_t slot = (uint32_t) floorf(u);
*slot_out = slot;
const float f = u - floorf(u);
const float life = 0.75f; // fraction of the period the burst is visible
if (f > life) return 0.0f;
const float age = f / life;
const float centre = focus + (rnd(seed, slot) - 0.5f) * span * 0.8f;
const float radius = 2.0f + 12.0f * sqrtf(age);
const float d = fabsf(wall - centre);
if (d > radius) return 0.0f;
const float ring = 0.35f + 0.65f * clamp01(1.0f - (radius - d) / 3.0f); // brighter rim
const float fade = (1.0f - age) * (1.0f - age);
return ring * fade;
}
// Colour of one LED for an effect. led is the LED index, wall the LED's
// position in wall inches, t seconds (wraps daily), focus the centre of the
// window lighting this LED and span its length in inches.
inline Rgbw paint(Effect e, int led, float wall, float t, float focus, float span) {
Rgbw c = {0, 0, 0, 0};
switch (e) {
case NONE:
case EFFECT_COUNT:
break;
case RAINBOW:
// One full rainbow per 30 in of stairs, drifting slowly downward.
c = hsv(wall / 30.0f - t / 12.0f, 1.0f, 1.0f);
break;
case NEW_YEARS: {
// Champagne gold with fast silver and gold confetti.
static const Rgbw GOLD = {1.0f, 0.62f, 0.12f, 0.35f};
static const Rgbw CONFETTI[3] = {{0.55f, 0.6f, 0.9f, 1.0f}, {1.0f, 0.75f, 0.2f, 0.6f}, {0, 0, 0, 1.0f}};
c = scale(GOLD, 0.45f + 0.15f * noise(11, wall / 8.0f + t * 0.6f));
uint32_t slot;
const float tw = twinkle(led, t, 0.7f, 0.35f, 12, &slot);
if (tw > 0.0f) c = mix(c, CONFETTI[hash(slot * 7u + led) % 3], tw);
break;
}
case VALENTINES: {
// Pink and red drifting together, pulsing with a heartbeat.
static const Rgbw PINK = {1.0f, 0.3f, 0.55f, 0.1f};
static const Rgbw RED = {1.0f, 0.02f, 0.12f, 0};
c = mix(PINK, RED, smoothstep(noise(21, wall / 14.0f - t * 0.25f) * 1.6f - 0.3f));
const float p = fract(t / 1.4f);
const float lub = expf(-p * 14.0f), dub = p > 0.22f ? 0.7f * expf(-(p - 0.22f) * 14.0f) : 0.0f;
c = scale(c, 0.5f + 0.5f * fminf(1.0f, lub + dub));
break;
}
case ST_PATRICKS: {
// Drifting greens with gold glints.
static const Rgbw GREENS[3] = {{0, 1.0f, 0.2f, 0}, {0.3f, 1.0f, 0, 0}, {0, 0.85f, 0.45f, 0}};
static const Rgbw GOLD = {1.0f, 0.65f, 0, 0.4f};
c = blocks(wall - t * 2.0f, 7.0f, 31, GREENS, 3, 0.4f);
c = mix(c, GOLD, twinkle(led, t, 1.6f, 0.18f, 32));
break;
}
case EASTER: {
// Slow pastel blocks.
static const Rgbw PASTELS[5] = {{1.0f, 0.4f, 0.6f, 0.15f}, {0.35f, 1.0f, 0.55f, 0.1f}, {0.7f, 0.45f, 1.0f, 0.1f},
{1.0f, 0.8f, 0.3f, 0.15f}, {0.4f, 0.7f, 1.0f, 0.1f}};
c = blocks(wall - t * 1.5f, 9.0f, 41, PASTELS, 5, 0.5f);
break;
}
case FOURTH_OF_JULY: {
// Night-sky navy with red, white and blue fireworks around the person.
static const Rgbw SKY = {0, 0.02f, 0.3f, 0};
static const Rgbw SPARK[3] = {{1.0f, 0.02f, 0.05f, 0}, {0.2f, 0.2f, 0.35f, 1.0f}, {0.15f, 0.35f, 1.0f, 0}};
c = SKY;
uint32_t slot;
float b = burst(wall, t, 1.7f, 51, focus, span, &slot);
if (b > 0.0f) c = lighten(c, scale(SPARK[slot % 3], b));
b = burst(wall, t + 0.6f, 2.3f, 52, focus, span, &slot);
if (b > 0.0f) c = lighten(c, scale(SPARK[(slot + 1) % 3], b));
const float tw = twinkle(led, t, 0.9f, 0.08f, 53);
if (tw > 0.0f) c = lighten(c, scale(SPARK[1], tw));
break;
}
case AUTUMN: {
// Rust, amber, gold and russet leaves drifting down the stairs.
static const Rgbw LEAVES[4] = {{0.85f, 0.2f, 0, 0}, {1.0f, 0.5f, 0, 0}, {1.0f, 0.75f, 0.08f, 0.1f},
{0.6f, 0.08f, 0.02f, 0}};
c = blocks(wall - t * 3.0f, 5.0f, 61, LEAVES, 4, 0.35f);
c = scale(c, 0.8f + 0.2f * noise(62, wall / 3.0f - t * 1.5f));
break;
}
case HALLOWEEN: {
// Jack-o'-lantern orange with candle flicker; purple patches drift
// through and the odd LED flashes slime green.
static const Rgbw ORANGE = {1.0f, 0.38f, 0, 0};
static const Rgbw PURPLE = {0.6f, 0, 1.0f, 0};
static const Rgbw SLIME = {0.35f, 1.0f, 0, 0};
const float m = smoothstep((noise(71, wall / 16.0f - t * 0.2f) - 0.5f) * 4.0f);
c = mix(ORANGE, PURPLE, m);
c = scale(c, 0.55f + 0.45f * noise2(72, t * 6.0f, wall / 4.0f)); // flickers in clumps of a few LEDs
c = mix(c, SLIME, twinkle(led, t, 2.5f, 0.05f, 73));
break;
}
case HAUNTED: {
// Cold ghost light that breathes, with a slow dark patch wandering
// through it, and every few seconds the whole thing stutters like a
// dying bulb.
static const Rgbw GHOST = {0.12f, 0.45f, 0.75f, 0.55f};
float v = 0.75f + 0.25f * sinf(t * 1.3f);
v *= 0.35f + 0.65f * smoothstep((fabsf(wall - (focus + 0.6f * span * sinf(t * 0.4f))) - 2.0f) / 5.0f);
const float cycle = 6.0f;
const uint32_t slot = (uint32_t) floorf(t / cycle);
const float local = t - slot * cycle - rnd(81, slot) * 4.5f;
if (rnd(82, slot) < 0.65f && local >= 0.0f && local < 1.1f) {
const uint32_t q = (uint32_t) (local * 18.0f);
if (rnd(83, slot * 64u + q) < 0.5f) v *= 0.06f;
}
c = scale(GHOST, v);
break;
}
case HANUKKAH: {
// Blue and white blocks with a soft shimmer and white sparkles.
static const Rgbw BLUE = {0, 0.2f, 1.0f, 0};
static const Rgbw WHITE = {0.25f, 0.3f, 0.55f, 0.9f};
c = mix(BLUE, WHITE, stripe(wall - t * 1.2f, 14.0f, 0.4f, 3.0f));
c = scale(c, 0.75f + 0.25f * noise(91, wall / 4.0f + t * 1.2f));
c = mix(c, WHITE, twinkle(led, t, 1.8f, 0.15f, 92));
break;
}
case CHRISTMAS: {
// Red and green bands marching down the stairs with warm-white twinkles.
static const Rgbw RED = {1.0f, 0.02f, 0.02f, 0};
static const Rgbw GREEN = {0, 1.0f, 0.12f, 0};
static const Rgbw TWINKLE = {0.25f, 0.18f, 0.05f, 1.0f};
c = mix(GREEN, RED, stripe(wall - t * 2.0f, 12.0f, 0.5f, 1.5f));
c = mix(c, TWINKLE, twinkle(led, t, 1.5f, 0.2f, 101));
break;
}
case CANDY_CANE: {
// Red and white stripes scrolling down.
static const Rgbw RED = {1.0f, 0, 0.04f, 0};
static const Rgbw WHITE = {0.2f, 0.2f, 0.2f, 1.0f};
c = mix(WHITE, RED, stripe(wall - t * 3.0f, 8.0f, 0.5f, 1.5f));
break;
}
case SNOWFALL: {
// Icy blue with snowflakes falling down the stairs.
static const Rgbw ICE = {0, 0.12f, 0.45f, 0.05f};
static const Rgbw FLAKE = {0.3f, 0.4f, 0.6f, 1.0f};
// Flakes fall from above the top to below the bottom and reappear at
// the top; their count keeps one flake per 8.6 in of fall on any stairs.
static const float MARGIN = 20.0f;
static const int MAX_FLAKES = 160;
const float loop = extent + 2.0f * MARGIN;
const int flakes = std::min(MAX_FLAKES, (int) (loop / 8.6f) + 1);
// Flake positions, computed once per frame.
static float flake_t = -1.0f, flake_pos[MAX_FLAKES], flake_v[MAX_FLAKES];
if (t != flake_t) {
flake_t = t;
for (int k = 0; k < flakes; k++) {
const float speed = 4.0f + 5.0f * rnd(111, k);
flake_pos[k] = fmodf(rnd(112, k) * loop + speed * t, loop) - MARGIN;
flake_v[k] = 0.55f + 0.45f * rnd(113, k);
}
}
c = ICE;
for (int k = 0; k < flakes; k++) {
const float d = fabsf(wall - flake_pos[k]);
if (d < 1.6f) c = lighten(c, scale(FLAKE, flake_v[k] * (1.0f - d / 1.6f)));
}
break;
}
case FIREPLACE: {
// Flames rising up the stairs: deep red embers to orange to yellow tips.
static const Rgbw EMBER = {0.45f, 0, 0, 0};
static const Rgbw FLAME = {1.0f, 0.32f, 0, 0};
static const Rgbw TIP = {1.0f, 0.72f, 0.1f, 0.35f};
float heat = 0.6f * noise(121, (wall + t * 10.0f) / 4.0f) + 0.4f * noise(122, (wall + t * 18.0f) / 1.5f);
heat = clamp01((heat - 0.2f) * 1.7f);
c = heat < 0.5f ? mix(EMBER, FLAME, heat * 2.0f) : mix(FLAME, TIP, (heat - 0.5f) * 2.0f);
break;
}
case AURORA: {
// Green, teal and violet curtains drifting slowly with a shimmer.
static const Rgbw GREEN = {0, 1.0f, 0.35f, 0};
static const Rgbw TEAL = {0, 0.7f, 0.85f, 0};
static const Rgbw VIOLET = {0.55f, 0.05f, 1.0f, 0};
const float h = noise(131, wall / 30.0f - t * 0.08f);
c = h < 0.5f ? mix(GREEN, TEAL, h * 2.0f) : mix(TEAL, VIOLET, (h - 0.5f) * 2.0f);
c = scale(c, 0.45f + 0.55f * noise(132, wall / 9.0f + t * 0.35f));
break;
}
case OCEAN: {
// Deep blue swells rolling down the stairs with the odd white cap.
static const Rgbw DEEP = {0, 0.06f, 0.6f, 0};
static const Rgbw SHALLOW = {0, 0.6f, 0.9f, 0};
static const Rgbw FOAM = {0.3f, 0.5f, 0.7f, 0.8f};
const float swell = 0.5f + 0.5f * sinf(6.2831853f * (wall / 40.0f - t / 7.0f));
const float n = noise(141, wall / 10.0f - t * 0.4f);
const float h = clamp01(0.65f * swell + 0.35f * n);
c = mix(DEEP, SHALLOW, h);
c = mix(c, FOAM, smoothstep((h - 0.82f) * 6.0f));
break;
}
}
// Gamma 2 per channel.
return {clamp01(c.r) * clamp01(c.r), clamp01(c.g) * clamp01(c.g), clamp01(c.b) * clamp01(c.b),
clamp01(c.w) * clamp01(c.w)};
}
} // namespace fx
} // namespace follow_me
firmware/follow-me-wizard.h
+5887
View File
File diff suppressed because it is too large. Load diff
firmware/follow-me.h
+269
View File
@@ -0,0 +1,269 @@
#pragma once
// Shared state for the follow-me light (follow-me.yaml): the per-person
// windows, the WLED watcher and the DDP sender.
//
// A FreeRTOS task talks to the WLED controller over plain blocking lwIP
// sockets, so the main loop never waits on the network. ESPHome's
// http_request component is not used because it blocks the main loop for the
// whole request (and for the full timeout while WLED is unreachable).
//
// The WLED address comes from the stored calibration or the LED preview
// (set_wled_host()); it may be an IPv4 address or a host name. The task:
// - resolves the address and reads /json/info for the LED count and
// whether the strip has a white channel, when the address changes and
// every INFO_MS after that (so DHCP changes and strip edits are picked up);
// - polls /json/state every POLL_MS. wled_on is true while WLED reports
// itself on. When pause_when_on() is set, the follow-me loop stops
// streaming while WLED is on (someone is using the light), and on each
// off-to-on change the task posts {"live":false} so WLED leaves realtime
// mode at once. An unreachable WLED reads as off.
// wled_ip is 0 and wled_leds 0 until the controller has answered.
//
// Frames go to WLED over DDP (UDP port 4048): 3 bytes per LED on RGB strips,
// 4 on RGBW, split over as many packets as the strip needs. DDP addresses
// LEDs by their raw index on the controller, like WLED's other realtime
// protocols, independent of WLED's segments.
#include <algorithm>
#include <atomic>
#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <mutex>
#include "esphome/components/json/json_util.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "lwip/inet.h"
#include "lwip/netdb.h"
#include "lwip/sockets.h"
namespace follow_me {
inline std::atomic<bool> wled_on{false};
inline std::atomic<uint32_t> wled_ip{0}; // resolved IPv4, network byte order; 0 = unknown
inline std::atomic<int> wled_leds{0}; // LED count WLED reports; 0 = unknown
inline std::atomic<bool> wled_white{false}; // strip has a white channel (RGBW)
inline std::atomic<bool> pause_when_on{true}; // mirrors the "Follow-Me Pause While WLED On" switch
static const uint32_t POLL_MS = 500;
static const uint32_t INFO_MS = 30000;
static const uint32_t RELEASE_DELAY_MS = 60; // lets the loop stop streaming before the release
static const size_t HOST_MAX = 128;
inline char wled_host[HOST_MAX] = "";
inline uint32_t wled_host_gen = 0; // bumped on every change
inline std::mutex wled_host_mutex;
inline void set_wled_host(const char *host) {
std::lock_guard<std::mutex> lock(wled_host_mutex);
if (strncmp(wled_host, host, HOST_MAX) == 0) return;
strncpy(wled_host, host, HOST_MAX - 1);
wled_host[HOST_MAX - 1] = '\0';
wled_host_gen++;
}
// IPv4 address of host (dotted quad or name), network byte order; 0 on failure.
inline uint32_t resolve(const char *host) {
struct in_addr a;
if (inet_aton(host, &a)) return a.s_addr;
struct addrinfo hints = {};
hints.ai_family = AF_INET;
struct addrinfo *res = nullptr;
if (lwip_getaddrinfo(host, nullptr, &hints, &res) != 0 || res == nullptr) return 0;
const uint32_t ip = reinterpret_cast<struct sockaddr_in *>(res->ai_addr)->sin_addr.s_addr;
lwip_freeaddrinfo(res);
return ip;
}
// One HTTP/1.0 exchange with WLED's web server (port 80, plain HTTP; WLED
// serves nothing else). Reads at most len - 1 bytes of the response into buf
// (NUL-terminated) and returns a pointer to the body, or nullptr on failure.
inline const char *http_exchange(uint32_t ip, const char *request, char *buf, size_t len) {
const int s = lwip_socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
if (s < 0) return nullptr;
struct timeval tv = {1, 0};
lwip_setsockopt(s, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv));
lwip_setsockopt(s, SOL_SOCKET, SO_SNDTIMEO, &tv, sizeof(tv));
struct sockaddr_in addr = {};
addr.sin_family = AF_INET;
addr.sin_port = htons(80);
addr.sin_addr.s_addr = ip;
int got = -1;
const int request_len = strlen(request);
if (lwip_connect(s, (struct sockaddr *) &addr, sizeof(addr)) == 0 &&
lwip_send(s, request, request_len, 0) == request_len) {
got = 0;
while (got < (int) len - 1) {
const int n = lwip_recv(s, buf + got, len - 1 - got, 0);
if (n <= 0) break;
got += n;
}
buf[got] = '\0';
}
lwip_close(s);
if (got <= 0) return nullptr;
const char *body = strstr(buf, "\r\n\r\n");
return body ? body + 4 : nullptr;
}
inline void watch_task(void *) {
static char buf[4096]; // /json/info is about 1.5 kB with its headers
char host[HOST_MAX] = "";
char get[HOST_MAX + 64];
uint32_t gen = 0, ip = 0, last_info = 0;
bool info_ok = false;
for (;;) {
{
std::lock_guard<std::mutex> lock(wled_host_mutex);
if (gen != wled_host_gen) {
gen = wled_host_gen;
memcpy(host, wled_host, HOST_MAX);
ip = 0;
info_ok = false;
wled_ip = 0;
wled_leds = 0;
}
}
if (!host[0]) {
wled_on = false;
vTaskDelay(pdMS_TO_TICKS(POLL_MS));
continue;
}
const uint32_t now = xTaskGetTickCount() * portTICK_PERIOD_MS;
if (!info_ok || now - last_info >= INFO_MS) {
last_info = now;
ip = resolve(host);
info_ok = false;
if (ip) {
snprintf(get, sizeof(get), "GET /json/info HTTP/1.0\r\nHost: %s\r\n\r\n", host);
if (const char *body = http_exchange(ip, get, buf, sizeof(buf))) {
esphome::json::parse_json(reinterpret_cast<const uint8_t *>(body), strlen(body), [&](JsonObject o) -> bool {
const int count = o["leds"]["count"] | 0;
const int lc = o["leds"]["lc"] | -1; // light capabilities: bit 0 RGB, bit 1 white
if (count <= 0 || lc < 0) return false;
wled_leds = count;
wled_white = (lc & 2) != 0;
info_ok = true;
return true;
});
}
}
wled_ip = info_ok ? ip : 0;
}
bool on = false;
if (info_ok) {
snprintf(get, sizeof(get), "GET /json/state HTTP/1.0\r\nHost: %s\r\n\r\n", host);
if (const char *body = http_exchange(ip, get, buf, sizeof(buf))) {
esphome::json::parse_json(reinterpret_cast<const uint8_t *>(body), strlen(body), [&](JsonObject o) -> bool {
on = o["on"] | false;
return true;
});
}
}
const bool was_on = wled_on.exchange(on);
if (on && !was_on && pause_when_on.load()) {
vTaskDelay(pdMS_TO_TICKS(RELEASE_DELAY_MS));
char release[HOST_MAX + 128];
snprintf(release, sizeof(release),
"POST /json/state HTTP/1.0\r\nHost: %s\r\nContent-Type: application/json\r\n"
"Content-Length: 14\r\n\r\n{\"live\":false}",
host);
http_exchange(ip, release, buf, sizeof(buf));
}
vTaskDelay(pdMS_TO_TICKS(POLL_MS));
}
}
// One lit window following one person. Positions are wall inches.
struct Track {
bool alive = false; // slot in use (lit, fading, or person lingering off the stairs)
bool tracking = false; // window has a filtered position
bool on_stairs = false; // the person has been on the stairs
bool entering = false; // entrance spring running
bool found = false; // matched to a radar target this frame
int exit_dir = 0; // left the stairs: 1 past the bottom, -1 past the top
float target = 0.0f; // filtered person position
float est_v = 0.0f; // estimated speed, inches/s
float last_z = 0.0f; // last radar position used
float centre = 0.0f; // window centre before speed limit and easing
float limited = 0.0f; // speed-limited window centre
float pos = 0.0f; // displayed window centre
float level = 0.0f; // fade envelope, 0 dark to 1 full
float exit_far = 0.0f; // farthest radar distance from the exit end since leaving
float ent_v = 0.0f; // entrance spring velocity, inches/s
uint32_t last_meas = 0;
uint32_t last_seen = 0;
uint32_t ent_start = 0;
// Matched radar target this frame.
float det_p = 0.0f, det_raw = 0.0f, det_x = 0.0f, det_y = 0.0f;
};
static const int MAX_TRACKS = 3; // the LD2450 tracks three targets
inline Track tracks[MAX_TRACKS];
// Centre of the brightest window, or NAN when all are dark.
inline float display_pos() {
float best = 0.0f, pos = NAN;
for (const Track &t : tracks) {
if (t.level > best) {
best = t.level;
pos = t.pos;
}
}
return pos;
}
// ---------- DDP ----------
static const uint16_t DDP_PORT = 4048;
static const int DDP_MAX_DATA = 1440; // bytes per packet; keeps each datagram inside one Ethernet frame
// Sends one frame (bpl bytes per LED, 3 or 4) to WLED over DDP, in as many
// packets as needed; the last carries the push flag. Main loop only.
inline void send_ddp(uint32_t ip, const uint8_t *data, int len, int bpl) {
static int sock = -1;
static uint8_t seq = 0;
if (sock < 0) {
sock = lwip_socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
if (sock < 0) return;
}
struct sockaddr_in dest = {};
dest.sin_family = AF_INET;
dest.sin_port = htons(DDP_PORT);
dest.sin_addr.s_addr = ip;
seq = seq % 15 + 1; // 1-15; 0 means "no sequence"
const int chunk = DDP_MAX_DATA / bpl * bpl;
static uint8_t pkt[10 + DDP_MAX_DATA];
for (int off = 0; off < len; off += chunk) {
const int n = std::min(chunk, len - off);
const bool last = off + n >= len;
pkt[0] = 0x40 | (last ? 0x01 : 0x00); // version 1, push on the last packet
pkt[1] = seq;
pkt[2] = bpl == 4 ? 0x1B : 0x0B; // RGBW or RGB, 8 bits per channel
pkt[3] = 1; // destination: default output device
pkt[4] = off >> 24;
pkt[5] = off >> 16;
pkt[6] = off >> 8;
pkt[7] = off;
pkt[8] = n >> 8;
pkt[9] = n;
memcpy(pkt + 10, data + off, n);
if (lwip_sendto(sock, pkt, 10 + n, 0, (struct sockaddr *) &dest, sizeof(dest)) < 0) {
lwip_close(sock);
sock = -1;
return;
}
}
}
// Starts the watcher once.
inline void start_watch() {
static bool started = false;
if (started) return;
started = true;
xTaskCreate(watch_task, "follow_me_wled", 6144, nullptr, 1, nullptr);
}
} // namespace follow_me
firmware/follow-me.yaml
+988
View File
@@ -0,0 +1,988 @@
# Follow-me stair light. ESPHome package for an LD2450 radar device such as
# the Apollo MTR-1 (ESP32, esp-idf, ESPHome 2026.8.2 or newer, web_server).
#
# Reads the radar's three tracked targets, keeps the ones on the stairs, and
# streams a lit window around each person's position to a WLED controller over
# DDP (UDP port 4048), 4 bytes per LED on strips with a white channel and 3
# otherwise, as WLED reports. The stream lights the strip only while it is
# being sent; WLED returns to its own state after its realtime timeout.
#
# Everything about the stairs, the strip and the WLED address comes from the
# calibration the setup wizard stores on the device
# (follow-me-calibration.h). The device serves the wizard at /followme, and
# the stock web page links to it. The LED count and strip type come from WLED
# (follow-me.h). "Follow-Me Setup" reports what is missing: "No web server",
# "Not set up", "Radar not found", "WLED not found", or "Ready".
#
# Radar targets are found by name (substitutions radar_x_name, radar_y_name;
# Apollo's names by default).
#
# Manual control: with "Follow-Me Pause While WLED On" set (the default),
# while the WLED controller itself is on (turned on from Home Assistant, the
# WLED app or its button), follow-me sends nothing and the strip shows WLED's
# own state. follow-me.h polls WLED's on/off on a background task and makes
# WLED leave realtime mode the moment it turns on; follow-me resumes, starting
# fresh, once WLED is off again. "Follow-Me Paused" shows WLED's on/off. An
# unreachable WLED counts as off. With the switch cleared, follow-me streams
# whatever WLED's own state is.
#
# Behaviour:
# - Positions are wall inches: inches along the stairs, 0 at the top edge
# of the top step, increasing down the stairs. Each target is projected
# onto its nearest point on the calibrated walking path. Targets farther
# than GATE_WIDTHS stair widths from the path (GATE_END_STEPS steps beyond
# either end of it) are ignored: people on the floors above and below,
# and reflections.
# - Distances scale with the staircase: "steps" are the calibration's mean
# tread run. "Follow-Me Window", "Follow-Me Buffer", "Follow-Me Night Light
# Size", "Follow-Me Offset" and "Follow-Me Approach" are in steps, as are
# the internal ranges.
# - Approach: a person off the stairs within "Follow-Me Approach" steps of
# either end (2 by default) is followed, with the window held at that end
# of the stairs, so the light is on before they step onto the stairs;
# farther out they are ignored.
# - Gaps: where the strip has a gap (a jumper across a landing, a corner),
# windows are drawn with the gap squeezed to the strip's usual LED
# spacing, so a window crossing it lights both sides
# (follow-me-calibration.h).
# - "Follow-Me Offset" puts the window that many steps ahead of the person
# in the direction they walk (higher going up, lower going down; negative
# values put it behind), 0 by default. It blends in with walking speed,
# reaching full strength at OFFSET_FULL_SPEED, so standing still has no
# shift.
# - Motion: an alpha-beta filter estimates position and walking speed from
# the radar frames, and the position is taken LEAD seconds ahead of the
# estimate (cancelling radar latency). "Follow-Me Smoothness" sets the
# filter and easing weight: 0% follows the radar closely, 100% is slow and
# floaty.
# - The light is a solid window "Follow-Me Window" steps long. The window
# stays still while the person is inside it and slides only when the
# person comes within "Follow-Me Buffer" steps of either end, by just
# enough to keep that margin. Its edges are anti-aliased, so only the
# first and last LED are ever partly lit. The window never moves faster
# than MAX_SPEED and its motion is eased. Only the stairs are lit: parts
# of the window past either end of the stairs or the strip stay dark.
# - Arrival: when the light turns on with the person within ENTRANCE_STEPS
# steps of either end of the stairs (at most ENTRANCE_MAX_FRACTION of
# their length), the window enters from that end and springs to the
# person (underdamped, about 20% overshoot), fading in over ENTRANCE_FADE
# seconds. Anywhere else it appears centred on the person and fades in
# over "Follow-Me Fade In" seconds. On loss it holds for "Follow-Me Hold"
# seconds, then fades out in place.
# - Leaving: once a person who was on the stairs is past either end, there
# is no hold; the window shrinks toward that end while fading out over
# EXIT_FADE seconds. It stays dark while that person lingers off the
# stairs, and arrives again when they step back on, move REARM_STEPS back
# toward the stairs from the farthest point they reached, or have been out
# of sight for "Follow-Me Hold" seconds. A person first seen off the
# stairs arrives as usual. Fades follow a smoothstep curve.
# - People: with "Follow-Me Multi-Person" off, one window follows one
# person (the target nearest the current window). With it on, up to three
# windows follow up to three people; radar targets are matched to windows
# by nearest position (at most ASSOC_JUMP_S at MAX_SPEED apart), and where
# windows overlap each LED takes the brighter. In multi-person mode a new
# window starts only for a target near either end of the stairs (the
# entrance range) and at least DUP_GAP_STEPS from every existing window,
# so reflections do not get one; a person the radar loses mid-stairs for
# longer than "Follow-Me Hold" is picked up again only near an end.
# - Look: the "Follow-Me Glow" light sets on/off (follow-me enabled),
# brightness and colour. It is an RGBW light so a white channel is its own
# control; the default is white only at 40%. All windows share it. Its
# channel values are the window's peak; ESPHome's gamma is off and the
# window edges use gamma 2.2 (see wled/README.md). No trail, no
# dithering. On a strip without a white channel the white channel is mixed
# from red, green and blue as "Follow-Me RGB White" picks (warm, neutral
# or cool).
# - Effects: picking an effect on the "Follow-Me Glow" light (Rainbow,
# holiday themes, Fireplace, Aurora, Ocean and others) replaces its colour
# with a moving per-LED pattern at the light's brightness, inside the same
# windows, fades and entrance. The patterns live in follow-me-effects.h
# and are fixed to the stairs, so a person walks through them;
# firework-style ones burst inside the person's window. The effects
# themselves do nothing in ESPHome; the stream paints them. Turning the
# light off resets the effect to None (ESPHome behaviour). At boot, an
# effect name without a pattern is logged as an error.
# - Night light: while the "Follow-Me Night Light" light is on, a patch
# "Follow-Me Night Light Size" steps long is lit at each end of the
# stairs, with a raised-cosine fade over NL_EDGE_STEPS on its inner side.
# It is its own RGBW light (default white only at 5%, 1 s transitions)
# and runs whether or not follow-me is enabled. Windows draw over it: each
# LED channel takes the brighter of the window and the night light.
# Scheduling it is left to Home Assistant.
# - LED preview: while the setup wizard asks for it (/followme/preview), the
# stream shows only the LED being matched, overriding everything above.
#
# Strip: the wizard's LED anchors pin LED numbers to spots along the path;
# LED positions are interpolated between anchors and extrapolated past the
# outer ones, so strips in pieces, with gaps, or running in either direction
# map correctly. LED numbers are raw indexes on the WLED controller, as DDP
# addresses them, independent of WLED's segments. Up to MAX_STREAM_LEDS LEDs.
esphome:
includes:
- ${follow_me_dir}/follow-me.h
- ${follow_me_dir}/follow-me-effects.h
- ${follow_me_dir}/follow-me-wizard.h
- ${follow_me_dir}/follow-me-calibration.h
# Adds a "Set up Follow-Me" link to the stock web page, opening the setup
# wizard served at /followme. Merges with the device's own web_server.
# The stock web page, served from the device so it works without internet,
# with a "Set up Follow-Me" button that opens the wizard (/followme).
web_server:
local: true
js_include: ${follow_me_dir}/follow-me-button.js
substitutions:
# Folder holding the follow-me headers, relative to the device file
# (ESPHome resolves includes from the device file, not from this package).
follow_me_dir: .
# Names of the radar's target X/Y sensors (raw state in mm); %d is the
# target number, 1-3. The defaults are Apollo's.
radar_x_name: "Target-%d X"
radar_y_name: "Target-%d Y"
globals:
# Follow-Me Glow channel values, 0-1.
- id: fm_red
type: float
restore_value: no
initial_value: "0"
- id: fm_green
type: float
restore_value: no
initial_value: "0"
- id: fm_blue
type: float
restore_value: no
initial_value: "0"
- id: fm_white
type: float
restore_value: no
initial_value: "0"
# Follow-Me Night Light channel values, 0-1.
- id: nl_red
type: float
restore_value: no
initial_value: "0"
- id: nl_green
type: float
restore_value: no
initial_value: "0"
- id: nl_blue
type: float
restore_value: no
initial_value: "0"
- id: nl_white
type: float
restore_value: no
initial_value: "0"
output:
# Follow-Me Glow channel values (brightness applied), read by the stream.
- platform: template
id: fm_out_r
type: float
write_action:
- lambda: id(fm_red) = state;
- platform: template
id: fm_out_g
type: float
write_action:
- lambda: id(fm_green) = state;
- platform: template
id: fm_out_b
type: float
write_action:
- lambda: id(fm_blue) = state;
- platform: template
id: fm_out_w
type: float
write_action:
- lambda: id(fm_white) = state;
# Follow-Me Night Light channel values (brightness applied).
- platform: template
id: nl_out_r
type: float
write_action:
- lambda: id(nl_red) = state;
- platform: template
id: nl_out_g
type: float
write_action:
- lambda: id(nl_green) = state;
- platform: template
id: nl_out_b
type: float
write_action:
- lambda: id(nl_blue) = state;
- platform: template
id: nl_out_w
type: float
write_action:
- lambda: id(nl_white) = state;
light:
- platform: rgbw
id: follow_me_glow
name: "Follow-Me Glow"
icon: mdi:stairs
red: fm_out_r
green: fm_out_g
blue: fm_out_b
white: fm_out_w
color_interlock: false
gamma_correct: 1.0
default_transition_length: 500ms
restore_mode: RESTORE_DEFAULT_ON
initial_state:
state: true
color_mode: RGB_WHITE
brightness: 40%
color_brightness: 0% # RGB off; ESPHome normalises an all-zero red/green/blue to white
white: 100%
# Effects only name a pattern; the stream paints it (follow-me-effects.h,
# whose EFFECT_NAMES must match these names).
effects:
- lambda:
name: "Rainbow"
update_interval: 1h
lambda: "// Painted by the follow-me stream."
- lambda:
name: "New Year's"
update_interval: 1h
lambda: "// Painted by the follow-me stream."
- lambda:
name: "Valentine's"
update_interval: 1h
lambda: "// Painted by the follow-me stream."
- lambda:
name: "St. Patrick's"
update_interval: 1h
lambda: "// Painted by the follow-me stream."
- lambda:
name: "Easter"
update_interval: 1h
lambda: "// Painted by the follow-me stream."
- lambda:
name: "Fourth of July"
update_interval: 1h
lambda: "// Painted by the follow-me stream."
- lambda:
name: "Autumn"
update_interval: 1h
lambda: "// Painted by the follow-me stream."
- lambda:
name: "Halloween"
update_interval: 1h
lambda: "// Painted by the follow-me stream."
- lambda:
name: "Haunted"
update_interval: 1h
lambda: "// Painted by the follow-me stream."
- lambda:
name: "Hanukkah"
update_interval: 1h
lambda: "// Painted by the follow-me stream."
- lambda:
name: "Christmas"
update_interval: 1h
lambda: "// Painted by the follow-me stream."
- lambda:
name: "Candy Cane"
update_interval: 1h
lambda: "// Painted by the follow-me stream."
- lambda:
name: "Snowfall"
update_interval: 1h
lambda: "// Painted by the follow-me stream."
- lambda:
name: "Fireplace"
update_interval: 1h
lambda: "// Painted by the follow-me stream."
- lambda:
name: "Aurora"
update_interval: 1h
lambda: "// Painted by the follow-me stream."
- lambda:
name: "Ocean"
update_interval: 1h
lambda: "// Painted by the follow-me stream."
- platform: rgbw
id: follow_me_night_light
name: "Follow-Me Night Light"
icon: mdi:weather-night
red: nl_out_r
green: nl_out_g
blue: nl_out_b
white: nl_out_w
color_interlock: false
gamma_correct: 1.0
default_transition_length: 1s
restore_mode: RESTORE_DEFAULT_OFF
initial_state:
state: false
color_mode: RGB_WHITE
brightness: 5%
color_brightness: 0% # RGB off; ESPHome normalises an all-zero red/green/blue to white
white: 100%
switch:
- platform: template
id: follow_me_multi
name: "Follow-Me Multi-Person"
icon: mdi:account-multiple
entity_category: config
optimistic: true
restore_mode: RESTORE_DEFAULT_OFF
- platform: template
id: follow_me_pause_on
name: "Follow-Me Pause While WLED On"
icon: mdi:hand-back-right
entity_category: config
optimistic: true
restore_mode: RESTORE_DEFAULT_ON
select:
- platform: template
id: follow_me_rgb_white
name: "Follow-Me RGB White"
icon: mdi:palette
entity_category: config
options: ["Warm", "Neutral", "Cool"]
initial_option: "Neutral"
optimistic: true
restore_value: true
number:
- platform: template
id: follow_me_night_light_size
name: "Follow-Me Night Light Size"
icon: mdi:arrow-expand-horizontal
entity_category: config
unit_of_measurement: "steps"
mode: box
min_value: 0.5
max_value: 8
step: 0.5
initial_value: 2
optimistic: true
restore_value: true
- platform: template
id: follow_me_approach
name: "Follow-Me Approach"
icon: mdi:walk
entity_category: config
unit_of_measurement: "steps"
mode: box
min_value: 0
max_value: 6
step: 0.5
initial_value: 2
optimistic: true
restore_value: true
- platform: template
id: follow_me_window
name: "Follow-Me Window"
icon: mdi:arrow-expand-vertical
entity_category: config
unit_of_measurement: "steps"
mode: box
min_value: 1
max_value: 15
step: 0.5
initial_value: 3
optimistic: true
restore_value: true
- platform: template
id: follow_me_buffer
name: "Follow-Me Buffer"
icon: mdi:arrow-collapse-vertical
entity_category: config
unit_of_measurement: "steps"
mode: box
min_value: 0
max_value: 5
step: 0.25
initial_value: 1
optimistic: true
restore_value: true
- platform: template
id: follow_me_hold
name: "Follow-Me Hold"
icon: mdi:timer-sand
entity_category: config
unit_of_measurement: "s"
mode: box
min_value: 0
max_value: 60
step: 1
initial_value: 4
optimistic: true
restore_value: true
- platform: template
id: follow_me_offset
name: "Follow-Me Offset"
icon: mdi:arrow-up-down
entity_category: config
unit_of_measurement: "steps"
mode: box
min_value: -3
max_value: 3
step: 0.25
initial_value: 0
optimistic: true
restore_value: true
- platform: template
id: follow_me_smoothness
name: "Follow-Me Smoothness"
icon: mdi:waves
entity_category: config
unit_of_measurement: "%"
mode: slider
min_value: 0
max_value: 100
step: 5
initial_value: 50
optimistic: true
restore_value: true
- platform: template
id: follow_me_fade_in
name: "Follow-Me Fade In"
icon: mdi:transition
entity_category: config
unit_of_measurement: "s"
mode: box
min_value: 0.1
max_value: 10
step: 0.1
initial_value: 1.5
optimistic: true
restore_value: true
- platform: template
id: follow_me_fade_out
name: "Follow-Me Fade Out"
icon: mdi:transition
entity_category: config
unit_of_measurement: "s"
mode: box
min_value: 0.1
max_value: 10
step: 0.1
initial_value: 3.0
optimistic: true
restore_value: true
binary_sensor:
- platform: template
name: "Follow-Me Paused"
icon: mdi:hand-back-right
entity_category: diagnostic
lambda: return follow_me::wled_on.load();
sensor:
- platform: template
name: "Follow-Me Position"
icon: mdi:map-marker-path
entity_category: diagnostic
unit_of_measurement: "steps"
accuracy_decimals: 1
update_interval: 1s
lambda: |-
const float p = follow_me::display_pos();
return follow_me::geo.ready ? p / follow_me::geo.inches_per_step : NAN;
text_sensor:
- platform: template
name: "Follow-Me Setup"
icon: mdi:stairs-box
entity_category: diagnostic
update_interval: 5s
lambda: |-
if (!follow_me::web_ready) return {"No web server"};
if (!follow_me::geo.ready) return {"Not set up"};
if (follow_me::radar_missing) return {"Radar not found"};
if (follow_me::wled_leds.load() == 0) return {"WLED not found"};
return {"Ready"};
interval:
- interval: 20ms
then:
- lambda: |-
// Distances scale with the calibrated staircase: STEP is its mean
// tread run, WIDTH its width (both wall inches). Times and speeds are
// about people and the radar, the same everywhere.
static const float GATE_WIDTHS = 0.8f; // accepted distance from the path, in stair widths (half the stair plus radar wobble)
static const float GATE_END_STEPS = 4.0f; // accepted distance beyond either end of the path
static const float EXIT_FADE = 1.0f; // seconds, shrink and fade after leaving the stairs
static const float REARM_STEPS = 1.0f; // walking back toward the stairs this far (radar distance) ends lingering
static const float ENTRANCE_STEPS = 3.0f; // from an end of the stairs: the entrance animation and new multi-person windows
static const float ENTRANCE_MAX_FRACTION = 0.33f; // ... but at most this fraction of the stairs' length
static const float ENTRANCE_W = 8.0f; // entrance spring natural frequency, rad/s
static const float ENTRANCE_ZETA = 0.45f; // entrance spring damping (about 20% overshoot)
static const float ENTRANCE_FADE = 0.25f; // seconds
static const uint32_t ENTRANCE_MAX_MS = 1500;
static const float DUP_GAP_STEPS = 1.5f; // nearest a new window may start to an existing one
static const float NL_EDGE_STEPS = 0.6f; // night light patch soft edge
static const float GAMMA = 2.2f;
static const float MAX_SPEED = 60.0f; // inches/s (1.5 m/s): window speed cap, above a brisk stair walk
static const float ASSOC_JUMP_S = 1.0f; // multi-person: a window's person moves at most MAX_SPEED for this long between matches
static const float LEAD = 0.25f; // seconds taken ahead of the estimate (LD2450 latency)
static const float OFFSET_FULL_SPEED = 10.0f; // inches/s: "Follow-Me Offset" applies fully from this walking speed
static const float STALE_MS = 400; // no new radar frame for this long: speed decays
static const int TRAILING_ZEROS = 5; // dark frames sent after fading out
static const uint32_t KEEPALIVE_MS = 1000; // resend an unchanged frame this often (WLED leaves realtime mode after its timeout, 2.5 s by default)
// White channel as red, green, blue on strips without one ("Follow-Me RGB White").
static const float WHITE_MIX[3][3] = {{1.0f, 0.72f, 0.42f}, {1.0f, 0.88f, 0.74f}, {0.86f, 0.92f, 1.0f}};
// First run: stored calibration, its web endpoints, the WLED watcher,
// and a check that the effect names here match follow-me-effects.h.
static bool started = false;
if (!started) {
started = true;
follow_me::setup_calibration();
// The radar's target X/Y sensors, found by name
// (radar_x_name / radar_y_name, %d = target number).
for (int t = 0; t < follow_me::MAX_TRACKS; t++) {
char xn[64], yn[64];
snprintf(xn, sizeof(xn), "${radar_x_name}", t + 1);
snprintf(yn, sizeof(yn), "${radar_y_name}", t + 1);
for (auto *sens : App.get_sensors()) {
if (sens->get_name() == xn) follow_me::radar_x[t] = sens;
if (sens->get_name() == yn) follow_me::radar_y[t] = sens;
}
if (!follow_me::radar_x[t] || !follow_me::radar_y[t]) {
follow_me::radar_missing = true;
ESP_LOGE("follow_me", "No \"%s\" / \"%s\" sensor; set radar_x_name / radar_y_name", xn, yn);
}
}
for (auto *eff : id(follow_me_glow).get_effects()) {
const StringRef n = eff->get_name();
if (follow_me::fx::from_name(n.c_str(), n.size()) == follow_me::fx::NONE)
ESP_LOGE("follow_me", "Effect \"%s\" has no pattern in follow-me-effects.h", n.c_str());
}
}
follow_me::apply_pending();
follow_me::pause_when_on = id(follow_me_pause_on).state;
static uint32_t last_ms = 0;
static int zeros_sent = TRAILING_ZEROS;
follow_me::Track *tracks = follow_me::tracks;
const int MAX_TRACKS = follow_me::MAX_TRACKS;
const uint32_t now = millis();
float dt = last_ms == 0 ? 0.02f : (now - last_ms) / 1000.0f;
last_ms = now;
if (dt > 0.2f) dt = 0.2f;
static uint8_t data[follow_me::MAX_STREAM_LEDS * 4];
static uint8_t sent[sizeof(data)];
static int sent_len = 0;
static uint32_t sent_ip = 0, last_send = 0;
const bool net = network::is_connected();
// LED preview for the setup wizard: overrides everything else.
const int preview = follow_me::preview_now(now);
if (preview > 0) {
for (int i = 0; i < MAX_TRACKS; i++) tracks[i] = follow_me::Track();
zeros_sent = TRAILING_ZEROS;
const int leds = std::min(follow_me::wled_leds.load(), follow_me::MAX_STREAM_LEDS);
const uint32_t ip = follow_me::wled_ip.load();
if (!net || leds == 0 || ip == 0) return;
const int bpl = follow_me::wled_white.load() ? 4 : 3;
const int len = follow_me::preview_frame(data, leds, bpl, preview);
if (len == sent_len && ip == sent_ip && memcmp(data, sent, len) == 0 && now - last_send < KEEPALIVE_MS) return;
memcpy(sent, data, len);
sent_len = len;
sent_ip = ip;
last_send = now;
follow_me::send_ddp(ip, data, len, bpl);
return;
}
// Not set up, or WLED not found yet: send nothing.
const follow_me::Geometry &geo = follow_me::geo;
const int wled_leds = follow_me::wled_leds.load();
const uint32_t wled_ip = follow_me::wled_ip.load();
if (!geo.ready || wled_leds == 0 || wled_ip == 0) {
for (int i = 0; i < MAX_TRACKS; i++) tracks[i] = follow_me::Track();
zeros_sent = TRAILING_ZEROS;
return;
}
const bool white = follow_me::wled_white.load();
const int NUM_LEDS = std::min(wled_leds, follow_me::MAX_STREAM_LEDS);
const float *edge_wall = geo.edge_wall;
const float STEP = geo.inches_per_step;
const float WIDTH = 2.0f * geo.half_width;
const float STAIRS_TOP = 0.0f; // wall inches, top edge of the top step
const float STAIRS_BOTTOM = geo.stairs_bottom; // wall inches, front of the bottom step
// People off the stairs within "Follow-Me Approach" steps of an end
// are followed, their window held at that end; farther out they
// are ignored.
float approach_steps = id(follow_me_approach).state;
if (std::isnan(approach_steps) || approach_steps < 0.0f) approach_steps = 0.0f;
const float APPROACH = approach_steps * STEP;
const float POS_MIN = STAIRS_TOP, POS_MAX = STAIRS_BOTTOM;
const float GATE = GATE_WIDTHS * WIDTH;
const float GATE_END = GATE_END_STEPS * STEP;
const float REARM = REARM_STEPS * STEP;
const float ENTRANCE_RANGE = std::min(ENTRANCE_STEPS * STEP, ENTRANCE_MAX_FRACTION * STAIRS_BOTTOM);
const float DUP_GAP = DUP_GAP_STEPS * STEP;
const float NL_EDGE = NL_EDGE_STEPS * STEP;
const float ASSOC_GATE = MAX_SPEED * ASSOC_JUMP_S;
// The lit range is the stairs; LEDs past either end stay dark. The
// strip's ends, for the night light and the entrance, are taken
// within it.
// Drawing happens in geo.edge_draw coordinates: wall inches with the
// strip's gaps squeezed out (follow-me-calibration.h), so a window
// crossing a gap lights both sides of it.
const float *edge_draw = geo.edge_draw;
const float DRAW_TOP = follow_me::draw_pos(STAIRS_TOP), DRAW_BOTTOM = follow_me::draw_pos(STAIRS_BOTTOM);
float strip_lo = edge_draw[0], strip_hi = edge_draw[0];
for (int k = 1; k <= NUM_LEDS; k++) {
strip_lo = fminf(strip_lo, edge_draw[k]);
strip_hi = fmaxf(strip_hi, edge_draw[k]);
}
const float STRIP_TOP_END = fmaxf(DRAW_TOP, strip_lo);
const float STRIP_BOTTOM_END = fminf(DRAW_BOTTOM, strip_hi);
// The same ends in wall inches, for the entrance (tracking is in wall inches).
float wall_lo = edge_wall[0], wall_hi = edge_wall[0];
for (int k = 1; k <= NUM_LEDS; k++) {
wall_lo = fminf(wall_lo, edge_wall[k]);
wall_hi = fmaxf(wall_hi, edge_wall[k]);
}
const float ENTRY_TOP = fmaxf(STAIRS_TOP, wall_lo), ENTRY_BOTTOM = fminf(STAIRS_BOTTOM, wall_hi);
follow_me::fx::set_extent(STAIRS_BOTTOM);
// WLED is on and the pause switch is set: someone is using the
// light. Send nothing (not even dark frames, which would re-enter
// realtime mode) and start fresh when it turns off.
if (follow_me::pause_when_on.load() && follow_me::wled_on.load()) {
for (int i = 0; i < MAX_TRACKS; i++) tracks[i] = follow_me::Track();
zeros_sent = TRAILING_ZEROS;
return;
}
// Radar targets on the stairs path.
struct Det {
float p, raw, x, y;
bool used;
};
Det det[follow_me::MAX_TRACKS];
int nd = 0;
sensor::Sensor *const *xs = follow_me::radar_x, *const *ys = follow_me::radar_y;
for (int t = 0; t < MAX_TRACKS; t++) {
if (!xs[t] || !ys[t]) continue;
float x = xs[t]->get_raw_state(), y = ys[t]->get_raw_state();
if (std::isnan(x) || std::isnan(y)) continue;
x /= 25.4f; // mm to inches
y /= 25.4f;
float p = 0.0f;
bool at_end = false;
const float near_d2 = follow_me::project(x, y, &p, &at_end);
const float gate = at_end ? GATE_END : GATE;
if (near_d2 > gate * gate) continue;
if (p < STAIRS_TOP - APPROACH || p > STAIRS_BOTTOM + APPROACH) continue;
const float raw = p; // before clamping to the stairs
p = fminf(POS_MAX, fmaxf(POS_MIN, p));
det[nd++] = {p, raw, x, y, false};
}
// Match targets to windows.
const bool enabled = id(follow_me_glow).remote_values.is_on();
const bool multi = id(follow_me_multi).state;
const int slots = multi ? MAX_TRACKS : 1;
for (int i = 0; i < MAX_TRACKS; i++) tracks[i].found = false;
auto take = [&](follow_me::Track &tr, Det &d) {
tr.found = true;
tr.det_p = d.p;
tr.det_raw = d.raw;
tr.det_x = d.x;
tr.det_y = d.y;
d.used = true;
};
if (enabled) {
if (!multi) {
// One window: the target nearest it, or the first one.
int best = -1;
float best_d = 1e9f;
for (int j = 0; j < nd; j++) {
const float d = tracks[0].alive ? fabsf(det[j].p - tracks[0].target) : 0.0f;
if (d < best_d) {
best_d = d;
best = j;
}
}
if (best >= 0) take(tracks[0], det[best]);
} else {
// Nearest pairs first, within ASSOC_GATE.
for (int round = 0; round < MAX_TRACKS; round++) {
int bi = -1, bj = -1;
float bd = ASSOC_GATE;
for (int i = 0; i < slots; i++) {
if (!tracks[i].alive || tracks[i].found) continue;
for (int j = 0; j < nd; j++) {
if (det[j].used) continue;
const float d = fabsf(det[j].p - tracks[i].target);
if (d <= bd) {
bd = d;
bi = i;
bj = j;
}
}
}
if (bi < 0) break;
take(tracks[bi], det[bj]);
}
// New windows: near an end, clear of existing windows.
for (int j = 0; j < nd; j++) {
if (det[j].used) continue;
const bool near_end = det[j].raw >= STAIRS_BOTTOM - ENTRANCE_RANGE ||
det[j].raw <= STAIRS_TOP + ENTRANCE_RANGE;
if (!near_end) continue;
bool clear = true;
for (int i = 0; i < MAX_TRACKS; i++) {
if (tracks[i].alive && fabsf(det[j].p - tracks[i].target) < DUP_GAP) clear = false;
}
if (!clear) continue;
for (int i = 0; i < slots; i++) {
if (tracks[i].alive) continue;
tracks[i] = follow_me::Track();
tracks[i].alive = true;
take(tracks[i], det[j]);
break;
}
}
}
}
float smooth = id(follow_me_smoothness).state / 100.0f;
if (std::isnan(smooth)) smooth = 0.5f;
smooth = fminf(1.0f, fmaxf(0.0f, smooth));
const float alpha = 0.6f - 0.35f * smooth; // filter gain on position
const float beta = alpha * alpha / (2.0f - alpha); // filter gain on speed
const float glide = 0.08f + 0.27f * smooth; // window easing time constant, s
const float length = fmaxf(1.0f, id(follow_me_window).state) * STEP;
const float margin = fmaxf(0.0f, id(follow_me_buffer).state) * STEP;
const float slack = fmaxf(0.0f, length / 2.0f - margin); // free travel from centre
// "Follow-Me Offset": steps ahead of the person in the direction they
// walk (negative: behind), blended in with walking speed so a pause
// or a turn does not make the window jump.
float offset_steps = id(follow_me_offset).state;
if (std::isnan(offset_steps)) offset_steps = 0.0f;
auto lead_offset = [&](float v) { return offset_steps * STEP * fmaxf(-1.0f, fminf(1.0f, v / OFFSET_FULL_SPEED)); };
const float hold_s = id(follow_me_hold).state;
const uint32_t hold_ms = (uint32_t) ((std::isnan(hold_s) || hold_s < 0.0f ? 0.0f : hold_s) * 1000.0f);
float max_level = 0.0f;
for (int i = 0; i < MAX_TRACKS; i++) {
follow_me::Track &tr = tracks[i];
if (!tr.alive && tr.found) tr.alive = true; // single-person window starting
if (!tr.alive) continue;
// Predict.
if (tr.tracking) {
tr.target += tr.est_v * dt;
if (now - tr.last_meas > STALE_MS) tr.est_v *= expf(-dt / 0.25f);
}
if (tr.found) {
if (!tr.tracking || tr.level <= 0.0f) {
// Start centred on the person.
tr.target = tr.det_p;
tr.est_v = 0.0f;
tr.last_z = tr.det_p;
tr.last_meas = now;
tr.centre = tr.det_p;
tr.limited = tr.det_p;
tr.pos = tr.det_p;
} else if (tr.det_p != tr.last_z) {
// Correct with a new radar frame.
float dtm = (now - tr.last_meas) / 1000.0f;
dtm = fminf(0.5f, fmaxf(0.05f, dtm));
const float r = tr.det_p - tr.target;
tr.target += alpha * r;
tr.est_v += beta * r / dtm;
tr.last_z = tr.det_p;
tr.last_meas = now;
}
tr.tracking = true;
tr.last_seen = now;
}
tr.est_v = fminf(MAX_SPEED, fmaxf(-MAX_SPEED, tr.est_v));
if (tr.found) {
const int side = tr.det_raw > STAIRS_BOTTOM ? 1 : (tr.det_raw < STAIRS_TOP ? -1 : 0);
if (side == 0) {
tr.on_stairs = true;
tr.exit_dir = 0;
} else if (tr.on_stairs) {
tr.on_stairs = false;
tr.exit_dir = side;
tr.exit_far = 0.0f;
}
if (tr.exit_dir != 0) {
// Lingering ends when the person heads back toward the stairs.
const int e = tr.exit_dir > 0 ? 0 : geo.points - 1; // path starts on the lower floor
const float dist = hypotf(tr.det_x - geo.x[e], tr.det_y - geo.y[e]);
tr.exit_far = fmaxf(tr.exit_far, dist);
if (dist < tr.exit_far - REARM) tr.exit_dir = 0;
}
} else if (now - tr.last_seen > hold_ms) {
tr.on_stairs = false;
tr.exit_dir = 0;
}
const bool lit = enabled && i < slots && tr.tracking && tr.exit_dir == 0 &&
(now - tr.last_seen) <= hold_ms;
if (lit && tr.level <= 0.0f) {
// Arrival near an end: enter from beyond that end of the strip.
const float half = length / 2.0f;
if (ENTRY_BOTTOM - tr.target <= ENTRANCE_RANGE) {
tr.pos = ENTRY_BOTTOM + half;
tr.entering = true;
} else if (tr.target - ENTRY_TOP <= ENTRANCE_RANGE) {
tr.pos = ENTRY_TOP - half;
tr.entering = true;
}
tr.ent_v = 0.0f;
tr.ent_start = now;
}
if (!lit) tr.entering = false;
const float fade_in = tr.entering ? ENTRANCE_FADE : fmaxf(0.1f, id(follow_me_fade_in).state);
const float fade_out = tr.exit_dir != 0 ? EXIT_FADE : fmaxf(0.1f, id(follow_me_fade_out).state);
tr.level = lit ? fminf(1.0f, tr.level + dt / fade_in) : fmaxf(0.0f, tr.level - dt / fade_out);
if (!lit && tr.level <= 0.0f) tr.tracking = false;
if (tr.tracking && tr.entering) {
// Entrance: underdamped spring toward the person, then hand over.
const float person = tr.target + tr.est_v * LEAD + lead_offset(tr.est_v);
const float acc = ENTRANCE_W * ENTRANCE_W * (person - tr.pos) - 2.0f * ENTRANCE_ZETA * ENTRANCE_W * tr.ent_v;
tr.ent_v += acc * dt;
tr.pos += tr.ent_v * dt;
if (now - tr.ent_start > ENTRANCE_MAX_MS || (fabsf(person - tr.pos) < 0.5f && fabsf(tr.ent_v) < 2.0f)) {
tr.entering = false;
tr.centre = tr.pos;
tr.limited = tr.pos;
}
} else if (tr.tracking) {
// Window centre moves only when the person pushes past the slack.
const float person = tr.target + tr.est_v * LEAD + lead_offset(tr.est_v);
const float e = person - tr.centre;
if (fabsf(e) > slack) tr.centre = person - copysignf(slack, e);
const float step = MAX_SPEED * dt;
tr.limited += fminf(step, fmaxf(-step, tr.centre - tr.limited));
tr.pos += (tr.limited - tr.pos) * (1.0f - expf(-dt / glide));
}
// Free the slot once dark and out of sight.
if (!tr.tracking && now - tr.last_seen > hold_ms) tr.alive = false;
max_level = fmaxf(max_level, tr.level);
}
if (!net) return;
const float nl[4] = {id(nl_red) * 255.0f, id(nl_green) * 255.0f, id(nl_blue) * 255.0f,
id(nl_white) * 255.0f};
const bool nl_on = nl[0] > 0.0f || nl[1] > 0.0f || nl[2] > 0.0f || nl[3] > 0.0f;
if (max_level <= 0.0f && !nl_on) {
if (zeros_sent >= TRAILING_ZEROS) return;
zeros_sent++;
} else {
zeros_sent = 0;
}
// Frame: 4 bytes per LED (RGBW) or 3 (RGB), sent over DDP.
// LED i covers the wall span between edges i and i + 1; its
// brightness is the largest fraction of that span inside any window,
// so only the end LEDs of a window are partial.
const int bpl = white ? 4 : 3;
const int frame_len = NUM_LEDS * bpl;
memset(data, 0, frame_len);
const float *white_mix = WHITE_MIX[std::min<size_t>(id(follow_me_rgb_white).active_index().value_or(1), 2)];
const float peak[4] = {id(fm_red) * 255.0f, id(fm_green) * 255.0f, id(fm_blue) * 255.0f,
id(fm_white) * 255.0f};
// Follow-Me Glow effect: replaces the colour with a per-LED pattern
// at the light's brightness (transitions included).
const StringRef fx_name = id(follow_me_glow).get_effect_name();
const follow_me::fx::Effect effect = follow_me::fx::from_name(fx_name.c_str(), fx_name.size());
const LightColorValues &glow_now = id(follow_me_glow).current_values;
const float fx_peak = glow_now.get_state() * glow_now.get_brightness() * 255.0f;
const float fx_t = (now % 86400000u) / 1000.0f; // wraps daily so float seconds keep ms resolution
// Each window's lit span; leaving windows shrink toward the end the
// person left by.
float win_lo[MAX_TRACKS], win_hi[MAX_TRACKS], win_eased[MAX_TRACKS];
for (int k = 0; k < MAX_TRACKS; k++) {
const follow_me::Track &tr = tracks[k];
win_eased[k] = tr.level * tr.level * (3.0f - 2.0f * tr.level); // smoothstep
const float centre = follow_me::draw_pos(tr.pos);
float lo = centre - length / 2.0f;
float hi = fminf(DRAW_BOTTOM, centre + length / 2.0f);
lo = fmaxf(lo, fminf(DRAW_TOP, hi));
if (tr.exit_dir > 0) {
lo = hi - (hi - lo) * tr.level;
} else if (tr.exit_dir < 0) {
lo = fmaxf(lo, STRIP_TOP_END);
hi = lo + (hi - lo) * tr.level;
}
win_lo[k] = lo;
win_hi[k] = hi;
}
const float nl_len = fmaxf(0.5f, id(follow_me_night_light_size).state) * STEP;
const float nl_edge = fminf(NL_EDGE, nl_len);
for (int i = 0; i < NUM_LEDS; i++) {
const float a = fminf(edge_draw[i], edge_draw[i + 1]), b = fmaxf(edge_draw[i], edge_draw[i + 1]);
float c = 0.0f;
int lit_by = 0; // window giving this LED its brightness
for (int k = 0; k < MAX_TRACKS; k++) {
if (win_eased[k] <= 0.0f) continue;
const float cover = (fminf(win_hi[k], b) - fmaxf(win_lo[k], a)) / (b - a);
if (cover > 0.0f && fminf(1.0f, cover) * win_eased[k] > c) {
c = fminf(1.0f, cover) * win_eased[k];
lit_by = k;
}
}
// Night light patch at each end of the strip.
float n = 0.0f;
const float mid = 0.5f * (a + b);
const float wall_mid = 0.5f * (edge_wall[i] + edge_wall[i + 1]); // effects are laid out in wall inches
if (nl_on && mid >= DRAW_TOP && mid <= DRAW_BOTTOM) {
const float d = fminf(mid - STRIP_TOP_END, STRIP_BOTTOM_END - mid); // inches from the nearer end
if (d <= nl_len - nl_edge) {
n = 1.0f;
} else if (d < nl_len) {
const float f = (d - (nl_len - nl_edge)) / nl_edge; // 0 at the patch, 1 at its edge
n = powf(0.5f + 0.5f * cosf(3.14159265f * f), GAMMA);
}
}
if (c <= 0.0f && n <= 0.0f) continue;
const float g = c > 0.0f ? powf(c, GAMMA) : 0.0f;
float led_peak[4] = {peak[0], peak[1], peak[2], peak[3]};
if (effect != follow_me::fx::NONE && c > 0.0f) {
const follow_me::fx::Rgbw col =
follow_me::fx::paint(effect, i, wall_mid, fx_t, tracks[lit_by].pos, length);
led_peak[0] = col.r * fx_peak;
led_peak[1] = col.g * fx_peak;
led_peak[2] = col.b * fx_peak;
led_peak[3] = col.w * fx_peak;
}
float v[4];
for (int ch = 0; ch < 4; ch++) v[ch] = fmaxf(led_peak[ch] * g, nl[ch] * n);
if (!white)
for (int ch = 0; ch < 3; ch++) v[ch] += v[3] * white_mix[ch];
for (int ch = 0; ch < bpl; ch++) {
const int q = (int) lroundf(v[ch]);
data[i * bpl + ch] = (uint8_t) (q > 255 ? 255 : q);
}
}
// Send only frames that changed, plus a keepalive.
if (frame_len == sent_len && wled_ip == sent_ip && memcmp(data, sent, frame_len) == 0 &&
now - last_send < KEEPALIVE_MS)
return;
memcpy(sent, data, frame_len);
sent_len = frame_len;
sent_ip = wled_ip;
last_send = now;
follow_me::send_ddp(wled_ip, data, frame_len, bpl);