312 lines
10 KiB
C
Executable File
312 lines
10 KiB
C
Executable File
/*
|
|
* proxvec.c -- Proximity Vector Engine implementation
|
|
*
|
|
* SPDX-License-Identifier: GPL-2.0-or-later
|
|
*
|
|
* Converts smoothed RSSI values from bledsp into abstract grid
|
|
* coordinates, tracks per-target velocity via a finite-state machine,
|
|
* and determines alarm state.
|
|
*
|
|
* Coordinate model:
|
|
* - Center of grid = the H2 device (the "holder")
|
|
* - Grid range: [0, PROXVEC_GRID_SIZE) in both axes
|
|
* - Center = (GRID_SIZE/2, GRID_SIZE/2)
|
|
* - Distance from center = "radius" in grid units
|
|
* - RSSI maps to radius via a log-distance path-loss model
|
|
* - Azimuth is pseudo-random at first detection (BLE has no
|
|
* directional info) and drifts slightly based on RSSI delta
|
|
* to give visual feedback of movement direction.
|
|
*/
|
|
|
|
#include "proxvec.h"
|
|
#include "log_manager.h"
|
|
|
|
#include <string.h>
|
|
#include <stdlib.h>
|
|
#include <math.h>
|
|
|
|
/* ------------------------------------------------------------------ */
|
|
/* Internal state */
|
|
/* ------------------------------------------------------------------ */
|
|
|
|
static proxvec_target_t targets[PROXVEC_MAX_TARGETS];
|
|
static int target_count;
|
|
static int alarm_radius = PROXVEC_ALARM_RADIUS;
|
|
static uint32_t update_tick;
|
|
|
|
/* ------------------------------------------------------------------ */
|
|
/* Helpers */
|
|
/* ------------------------------------------------------------------ */
|
|
|
|
/* Log-distance path-loss model (fixed-point, x10 to avoid floats).
|
|
*
|
|
* distance = 10 ^ ((TxPower - RSSI) / (10 * n))
|
|
*
|
|
* We compute this with a lookup table to avoid libm on the target.
|
|
* Table maps (TxPower - RSSI) in dB to a grid-radius value.
|
|
* Range: 0 dB (on top of us) to 80 dB (far away).
|
|
*
|
|
* grid_radius = scale * 10^((delta_dB * 10) / (10 * n * 10))
|
|
* = scale * 10^(delta_dB / n10)
|
|
* where n10 = PROXVEC_PATH_LOSS_N (= 25, representing n=2.5).
|
|
*
|
|
* We precompute: for delta_dB 0..80, radius = (GRID_SIZE/2 - 10) * (1 - dB/100)
|
|
* This is a linear approximation that's good enough for a tripwire alarm.
|
|
*/
|
|
#define DB_TO_GRID_MAX 80
|
|
|
|
static int8_t db_to_grid[DB_TO_GRID_MAX + 1];
|
|
|
|
static void build_db_table(void)
|
|
{
|
|
int max_r = PROXVEC_GRID_SIZE / 2 - 10;
|
|
for (int dB = 0; dB <= DB_TO_GRID_MAX; dB++) {
|
|
/* Linear mapping: 0 dB -> radius 5, 80 dB -> radius max_r */
|
|
int r = 5 + (max_r - 5) * dB / DB_TO_GRID_MAX;
|
|
if (r > max_r) r = max_r;
|
|
db_to_grid[dB] = (int8_t)r;
|
|
}
|
|
}
|
|
|
|
/* Convert RSSI to grid radius. Clamp to table bounds. */
|
|
static int rssi_to_radius(int8_t rssi)
|
|
{
|
|
int delta = PROXVEC_TX_POWER_DEFAULT / 10 - (int)rssi;
|
|
/* Tx is ~4 dBm, so delta = 4 - rssi.
|
|
* For rssi -40: delta = 44 -> close
|
|
* For rssi -90: delta = 94 -> far (clamped to 80) */
|
|
if (delta < 0) delta = 0;
|
|
if (delta > DB_TO_GRID_MAX) delta = DB_TO_GRID_MAX;
|
|
return (int)db_to_grid[delta];
|
|
}
|
|
|
|
/* Deterministic pseudo-random angle from MAC address.
|
|
* Uses a simple hash to spread devices around the circle. */
|
|
static int mac_to_angle(const uint8_t *mac)
|
|
{
|
|
uint32_t h = (uint32_t)mac[0] | ((uint32_t)mac[1] << 8) |
|
|
((uint32_t)mac[2] << 16) | ((uint32_t)mac[3] << 24);
|
|
h ^= h >> 16;
|
|
h *= 0x45d9f3b;
|
|
h ^= h >> 16;
|
|
return (int)(h % 360);
|
|
}
|
|
|
|
/* Find a target slot by MAC, or allocate a new one. */
|
|
static int find_target(const uint8_t *mac)
|
|
{
|
|
for (int i = 0; i < target_count; i++) {
|
|
if (memcmp(targets[i].mac, mac, 6) == 0)
|
|
return i;
|
|
}
|
|
if (target_count >= PROXVEC_MAX_TARGETS) {
|
|
/* Evict the oldest LOST target */
|
|
for (int i = 0; i < target_count; i++) {
|
|
if (targets[i].state == TRACK_LOST) {
|
|
memset(&targets[i], 0, sizeof(proxvec_target_t));
|
|
memcpy(targets[i].mac, mac, 6);
|
|
return i;
|
|
}
|
|
}
|
|
return -1;
|
|
}
|
|
int idx = target_count++;
|
|
memset(&targets[idx], 0, sizeof(proxvec_target_t));
|
|
memcpy(targets[idx].mac, mac, 6);
|
|
return idx;
|
|
}
|
|
|
|
/* Map RSSI delta to azimuth drift (degrees).
|
|
* A strengthening signal (negative delta) drifts the angle
|
|
* slightly to give visual impression of approach direction. */
|
|
static int delta_to_azimuth_drift(int8_t rssi_delta)
|
|
{
|
|
/* Clamp to [-5, +5] degrees per tick */
|
|
int drift = (int)rssi_delta / 4;
|
|
if (drift < -5) drift = -5;
|
|
if (drift > 5) drift = 5;
|
|
return -drift; /* invert: stronger signal -> negative delta -> positive drift */
|
|
}
|
|
|
|
/* Map smoothed RSSI to intensity [0, 255].
|
|
* -40 dBm (very close) -> 255; -100 dBm (far) -> 20 */
|
|
static uint8_t rssi_to_intensity(int8_t rssi)
|
|
{
|
|
int t = (rssi + 100) * 255 / 60;
|
|
if (t < 20) t = 20;
|
|
if (t > 255) t = 255;
|
|
return (uint8_t)t;
|
|
}
|
|
|
|
/* ------------------------------------------------------------------ */
|
|
/* Public API */
|
|
/* ------------------------------------------------------------------ */
|
|
|
|
void proxvec_init(void)
|
|
{
|
|
memset(targets, 0, sizeof(targets));
|
|
target_count = 0;
|
|
update_tick = 0;
|
|
build_db_table();
|
|
LOG_INF("proxvec: initialised (alarm_radius=%d)", alarm_radius);
|
|
}
|
|
|
|
void proxvec_update(const bledsp_device_t *devices, int count)
|
|
{
|
|
uint32_t now_ms;
|
|
struct timespec ts;
|
|
clock_gettime(CLOCK_MONOTONIC, &ts);
|
|
now_ms = (uint32_t)(ts.tv_sec * 1000U + ts.tv_nsec / 1000000U);
|
|
|
|
update_tick++;
|
|
|
|
/* Mark all existing targets as potentially lost */
|
|
for (int i = 0; i < target_count; i++) {
|
|
if (targets[i].state != TRACK_LOST) {
|
|
/* Will be re-confirmed below if device still active */
|
|
}
|
|
}
|
|
|
|
/* Update or create targets from the device table */
|
|
for (int d = 0; d < count; d++) {
|
|
int idx = find_target(devices[d].mac);
|
|
if (idx < 0) continue;
|
|
|
|
proxvec_target_t *t = &targets[idx];
|
|
t->prev_radius = t->radius;
|
|
t->age_ticks++;
|
|
|
|
/* Convert RSSI to grid radius */
|
|
t->radius = (int16_t)rssi_to_radius(devices[d].rssi_smoothed);
|
|
|
|
/* Compute radial velocity */
|
|
t->velocity = t->radius - t->prev_radius;
|
|
|
|
/* Compute x, y from radius and angle */
|
|
int base_angle;
|
|
if (t->age_ticks == 1) {
|
|
/* First detection: assign a pseudo-random angle from MAC */
|
|
t->x = PROXVEC_GRID_SIZE / 2; /* will be overwritten */
|
|
t->y = PROXVEC_GRID_SIZE / 2;
|
|
}
|
|
|
|
base_angle = mac_to_angle(devices[d].mac);
|
|
/* Apply azimuth drift based on RSSI delta */
|
|
base_angle += delta_to_azimuth_drift(devices[d].rssi_delta) * t->age_ticks;
|
|
base_angle = ((base_angle % 360) + 360) % 360;
|
|
|
|
/* Polar to cartesian, centered on grid */
|
|
int cx = PROXVEC_GRID_SIZE / 2;
|
|
int cy = PROXVEC_GRID_SIZE / 2;
|
|
double rad = (double)base_angle * 3.14159265 / 180.0;
|
|
t->x = cx + (int16_t)((double)t->radius * cos(rad));
|
|
t->y = cy + (int16_t)((double)t->radius * sin(rad));
|
|
|
|
/* Clamp to grid */
|
|
if (t->x < 0) t->x = 0;
|
|
if (t->y < 0) t->y = 0;
|
|
if (t->x >= PROXVEC_GRID_SIZE) t->x = PROXVEC_GRID_SIZE - 1;
|
|
if (t->y >= PROXVEC_GRID_SIZE) t->y = PROXVEC_GRID_SIZE - 1;
|
|
|
|
/* Intensity from RSSI */
|
|
t->intensity = rssi_to_intensity(devices[d].rssi_smoothed);
|
|
|
|
/* State machine transitions */
|
|
if (t->age_ticks == 1) {
|
|
t->state = TRACK_NEW;
|
|
} else {
|
|
switch (t->state) {
|
|
case TRACK_NEW:
|
|
t->state = TRACK_STATIONARY;
|
|
break;
|
|
case TRACK_STATIONARY:
|
|
if (t->velocity < -2) t->state = TRACK_APPROACHING;
|
|
else if (t->velocity > 2) t->state = TRACK_RECEDING;
|
|
break;
|
|
case TRACK_APPROACHING:
|
|
if (t->velocity >= 0) t->state = TRACK_STATIONARY;
|
|
else if (t->velocity > 2) t->state = TRACK_RECEDING;
|
|
break;
|
|
case TRACK_RECEDING:
|
|
if (t->velocity <= 0) t->state = TRACK_STATIONARY;
|
|
else if (t->velocity < -2) t->state = TRACK_APPROACHING;
|
|
break;
|
|
case TRACK_LOST:
|
|
t->state = TRACK_NEW; /* re-acquired */
|
|
t->age_ticks = 1;
|
|
break;
|
|
}
|
|
}
|
|
|
|
/* Alarm check */
|
|
t->alarm = (t->radius <= alarm_radius) ? 1 : 0;
|
|
}
|
|
|
|
/* Mark stale targets as LOST */
|
|
for (int i = 0; i < target_count; i++) {
|
|
if (targets[i].state != TRACK_LOST) {
|
|
int found = 0;
|
|
for (int d = 0; d < count; d++) {
|
|
if (memcmp(targets[i].mac, devices[d].mac, 6) == 0) {
|
|
found = 1;
|
|
break;
|
|
}
|
|
}
|
|
if (!found) {
|
|
/* Check stale timeout */
|
|
/* We don't store last_seen in target, use age as proxy */
|
|
if (targets[i].age_ticks > 3) { /* missed 3+ polls */
|
|
targets[i].state = TRACK_LOST;
|
|
targets[i].alarm = 0;
|
|
targets[i].intensity = 0;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
int proxvec_get_targets(proxvec_target_t *out, int max)
|
|
{
|
|
int written = 0;
|
|
for (int i = 0; i < target_count && written < max; i++) {
|
|
if (targets[i].state != TRACK_LOST) {
|
|
out[written++] = targets[i];
|
|
}
|
|
}
|
|
return written;
|
|
}
|
|
|
|
int proxvec_get_alarm_count(void)
|
|
{
|
|
int count = 0;
|
|
for (int i = 0; i < target_count; i++) {
|
|
if (targets[i].alarm) count++;
|
|
}
|
|
return count;
|
|
}
|
|
|
|
int proxvec_get_track_count(void)
|
|
{
|
|
int count = 0;
|
|
for (int i = 0; i < target_count; i++) {
|
|
if (targets[i].state != TRACK_LOST) count++;
|
|
}
|
|
return count;
|
|
}
|
|
|
|
void proxvec_reset(void)
|
|
{
|
|
memset(targets, 0, sizeof(targets));
|
|
target_count = 0;
|
|
update_tick = 0;
|
|
}
|
|
|
|
void proxvec_set_alarm_radius(int16_t r) { alarm_radius = r; }
|
|
int16_t proxvec_get_alarm_radius(void) { return alarm_radius; }
|
|
|
|
void proxvec_shutdown(void)
|
|
{
|
|
memset(targets, 0, sizeof(targets));
|
|
target_count = 0;
|
|
LOG_INF("proxvec: shut down");
|
|
} |