|
| 1 | +#include "presence.h" |
| 2 | + |
| 3 | +#include <string.h> |
| 4 | +#include <stdlib.h> |
| 5 | + |
| 6 | +#include "esp_log.h" |
| 7 | +#include "esp_timer.h" |
| 8 | +#include "freertos/FreeRTOS.h" |
| 9 | +#include "freertos/task.h" |
| 10 | +#include "freertos/semphr.h" |
| 11 | + |
| 12 | +#include "radar.h" |
| 13 | +#include "settings.h" |
| 14 | + |
| 15 | +static const char *TAG = "presence"; |
| 16 | + |
| 17 | +#define PRESENCE_TICK_MS 100 /* 10 Hz */ |
| 18 | +#define PRESENCE_STATIONARY_CMS 5 /* |v| < 5 cm/s == stationary */ |
| 19 | +#define VACANCY_DEFAULT_S 60 |
| 20 | +#define VACANCY_MIN_S 5 |
| 21 | +#define VACANCY_MAX_S 3600 |
| 22 | + |
| 23 | +static struct { |
| 24 | + presence_t latest; |
| 25 | + SemaphoreHandle_t lock; |
| 26 | + uint32_t vacancy_secs; |
| 27 | + /* Latest detection timestamp in microseconds since boot. |
| 28 | + * 0 means "never seen". */ |
| 29 | + uint64_t last_seen_us; |
| 30 | + bool inited; |
| 31 | + TaskHandle_t task; |
| 32 | +} s_pres; |
| 33 | + |
| 34 | +static void presence_task(void *arg) { |
| 35 | + (void)arg; |
| 36 | + while (1) { |
| 37 | + radar_frame_t f; |
| 38 | + esp_err_t r = radar_peek(&f); |
| 39 | + uint64_t now_us = (uint64_t)esp_timer_get_time(); |
| 40 | + |
| 41 | + bool any_now = false; |
| 42 | + bool any_stationary = false; |
| 43 | + int16_t nearest = -1; |
| 44 | + uint8_t count = 0; |
| 45 | + |
| 46 | + if (r == ESP_OK) { |
| 47 | + count = f.target_count; |
| 48 | + any_now = (f.target_count > 0) || f.present; |
| 49 | + |
| 50 | + /* `f.distance_cm` is the EUCLIDEAN distance the LD2450 driver |
| 51 | + * computes from the primary target's (x, y) — same number |
| 52 | + * the Live tab shows. The per-target `targets[i].y_cm` is |
| 53 | + * just the Y axis projection, NOT the radial distance, so |
| 54 | + * using it as "nearest" reports 0 when a target sits along |
| 55 | + * the X axis. (Bench-reproduced 2026-05-07.) |
| 56 | + * |
| 57 | + * For LD2410-family, distance_cm is just the moving or |
| 58 | + * stationary distance the radar reports. Same field, same |
| 59 | + * meaning. So both code paths reduce to: "if anyone is here, |
| 60 | + * nearest = f.distance_cm". */ |
| 61 | + if (any_now) { |
| 62 | + nearest = (int16_t)f.distance_cm; |
| 63 | + if (count == 0 && f.present) count = 1; |
| 64 | + |
| 65 | + /* Stationary detection: LD2450 reports per-target |
| 66 | + * velocity in cm/s; LD2410-family doesn't surface |
| 67 | + * velocity through the unified frame at all. Mark |
| 68 | + * stationary if any target has |v| under threshold, |
| 69 | + * OR if we don't know (LD2410 case — bias toward |
| 70 | + * stationary since the family is tuned for it). */ |
| 71 | + if (f.target_count > 0) { |
| 72 | + for (uint8_t i = 0; i < f.target_count && i < RADAR_MAX_TARGETS; ++i) { |
| 73 | + if (abs((int)f.targets[i].v_cms) < PRESENCE_STATIONARY_CMS) { |
| 74 | + any_stationary = true; |
| 75 | + break; |
| 76 | + } |
| 77 | + } |
| 78 | + } else { |
| 79 | + any_stationary = true; /* LD2410(C) — unknown velocity */ |
| 80 | + } |
| 81 | + |
| 82 | + s_pres.last_seen_us = now_us; |
| 83 | + } |
| 84 | + } |
| 85 | + |
| 86 | + uint64_t timeout_us = (uint64_t)s_pres.vacancy_secs * 1000000ULL; |
| 87 | + bool occupied = (s_pres.last_seen_us != 0) && |
| 88 | + ((now_us - s_pres.last_seen_us) < timeout_us); |
| 89 | + |
| 90 | + presence_t snap = { |
| 91 | + .occupied = occupied, |
| 92 | + .target_count = occupied ? count : 0, |
| 93 | + .stationary = occupied && any_stationary, |
| 94 | + .nearest_cm = occupied ? (nearest < 0 ? 0 : nearest) : -1, |
| 95 | + .last_seen_ms = (uint32_t)(s_pres.last_seen_us / 1000ULL), |
| 96 | + .ms_since_seen = (s_pres.last_seen_us == 0) ? UINT32_MAX |
| 97 | + : (uint32_t)((now_us - s_pres.last_seen_us) / 1000ULL), |
| 98 | + .vacancy_secs = s_pres.vacancy_secs, |
| 99 | + }; |
| 100 | + |
| 101 | + xSemaphoreTake(s_pres.lock, portMAX_DELAY); |
| 102 | + s_pres.latest = snap; |
| 103 | + xSemaphoreGive(s_pres.lock); |
| 104 | + |
| 105 | + vTaskDelay(pdMS_TO_TICKS(PRESENCE_TICK_MS)); |
| 106 | + } |
| 107 | +} |
| 108 | + |
| 109 | +esp_err_t presence_init(void) { |
| 110 | + if (s_pres.inited) return ESP_OK; |
| 111 | + s_pres.lock = xSemaphoreCreateMutex(); |
| 112 | + |
| 113 | + /* Load vacancy timeout from NVS or fall back to default. */ |
| 114 | + uint32_t v = 0; |
| 115 | + if (settings_get_u32("presence", "vacancy", &v) == ESP_OK && |
| 116 | + v >= VACANCY_MIN_S && v <= VACANCY_MAX_S) { |
| 117 | + s_pres.vacancy_secs = v; |
| 118 | + } else { |
| 119 | + s_pres.vacancy_secs = VACANCY_DEFAULT_S; |
| 120 | + } |
| 121 | + s_pres.last_seen_us = 0; |
| 122 | + /* Initial latest snapshot — vacant, never seen. */ |
| 123 | + s_pres.latest = (presence_t){ |
| 124 | + .occupied = false, .target_count = 0, .stationary = false, |
| 125 | + .nearest_cm = -1, .last_seen_ms = 0, .ms_since_seen = UINT32_MAX, |
| 126 | + .vacancy_secs = s_pres.vacancy_secs, |
| 127 | + }; |
| 128 | + |
| 129 | + BaseType_t ok = xTaskCreate(presence_task, "presence", 3072, NULL, 4, &s_pres.task); |
| 130 | + if (ok != pdPASS) { |
| 131 | + ESP_LOGE(TAG, "task create failed"); |
| 132 | + return ESP_FAIL; |
| 133 | + } |
| 134 | + s_pres.inited = true; |
| 135 | + ESP_LOGI(TAG, "presence detection up; vacancy timeout %u s", (unsigned)s_pres.vacancy_secs); |
| 136 | + return ESP_OK; |
| 137 | +} |
| 138 | + |
| 139 | +esp_err_t presence_get(presence_t *out) { |
| 140 | + if (!s_pres.inited || !out) return ESP_ERR_INVALID_STATE; |
| 141 | + xSemaphoreTake(s_pres.lock, portMAX_DELAY); |
| 142 | + *out = s_pres.latest; |
| 143 | + xSemaphoreGive(s_pres.lock); |
| 144 | + return ESP_OK; |
| 145 | +} |
| 146 | + |
| 147 | +esp_err_t presence_set_vacancy_timeout(uint32_t secs) { |
| 148 | + if (secs < VACANCY_MIN_S || secs > VACANCY_MAX_S) return ESP_ERR_INVALID_ARG; |
| 149 | + s_pres.vacancy_secs = secs; |
| 150 | + settings_set_u32("presence", "vacancy", secs); |
| 151 | + ESP_LOGI(TAG, "vacancy timeout set to %u s", (unsigned)secs); |
| 152 | + return ESP_OK; |
| 153 | +} |
| 154 | + |
| 155 | +uint32_t presence_get_vacancy_timeout(void) { |
| 156 | + return s_pres.inited ? s_pres.vacancy_secs : VACANCY_DEFAULT_S; |
| 157 | +} |
0 commit comments