feat: UWB tag BLE configuration interface (Issue #690)
Adds BLE GATT config server to uwb_tag firmware: Advertising: - Device name "UWB_TAG_XXXX" (last 4 hex digits of WiFi MAC) - Service UUID: 12345678-1234-5678-1234-56789abcdef0 - Compatible with nRF Connect app Characteristics: - Config (R/W) UUID: ...abcdef1 Read: returns current config as JSON Write: accepts partial JSON with any config keys - Status (R/N) UUID: ...abcdef2 Notifies "+OK" or "+ERR:<reason>" after each write Config keys (NVS-persisted, applied immediately unless noted): sleep_timeout_s [5..3600] OLED display timeout display_brightness [0..255] OLED contrast (SSD1306_SETCONTRAST) tag_name [max 16] friendly name uwb_channel [1..7] UWB RF channel (next boot) ranging_interval_ms [50..2000] minimum ranging poll interval battery_report bool include battery flag in ESP-NOW packets Partition: huge_app.csv (3MB app) — BLE + WiFi + DW1000 needs ~1.76MB Build: 55.8% flash, 18.1% RAM (SUCCESS) Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
sl-uwb
parent
6ea6e0ccd1
commit
1eda0c12a4
@ -14,6 +14,7 @@ lib_extra_dirs = ../../lib
|
||||
lib_deps =
|
||||
adafruit/Adafruit SSD1306@^2.5.7
|
||||
adafruit/Adafruit GFX Library@^1.11.5
|
||||
board_build.partitions = huge_app.csv
|
||||
build_flags =
|
||||
-DCORE_DEBUG_LEVEL=0
|
||||
-DTAG_ID=0x01
|
||||
|
||||
@ -27,6 +27,25 @@
|
||||
* I2C: SDA=4 SCL=5 OLED @0x3C, MPU6050 @0x68
|
||||
* E-Stop: GPIO 0 (BOOT), active LOW — disabled (floats LOW)
|
||||
* LED: GPIO 2
|
||||
*
|
||||
* BLE Configuration (Issue #690):
|
||||
* Advertises as "UWB_TAG_XXXX" (last 4 hex digits of MAC)
|
||||
* GATT service UUID: 12345678-1234-5678-1234-56789abcdef0
|
||||
* Config characteristic (R/W): 12345678-1234-5678-1234-56789abcdef1
|
||||
* Read: returns current config as JSON
|
||||
* Write: accepts JSON with any subset of config keys
|
||||
* Status characteristic (R/N): 12345678-1234-5678-1234-56789abcdef2
|
||||
* Notifies "+OK" or "+ERR:reason" after a write
|
||||
* Config persisted in NVS (survives reboot)
|
||||
* Compatible with nRF Connect app
|
||||
*
|
||||
* JSON config keys:
|
||||
* sleep_timeout_s int [5..3600] OLED sleep timeout (seconds)
|
||||
* display_brightness int [0..255] OLED contrast
|
||||
* tag_name str [max 16] friendly name (used in BLE name)
|
||||
* uwb_channel int [1..7] UWB RF channel (applied next boot)
|
||||
* ranging_interval_ms int [50..2000] minimum ranging poll interval
|
||||
* battery_report bool include battery % in ESP-NOW packets
|
||||
*/
|
||||
|
||||
#include <Arduino.h>
|
||||
@ -35,6 +54,11 @@
|
||||
#include <WiFi.h>
|
||||
#include <esp_now.h>
|
||||
#include <esp_wifi.h>
|
||||
#include <Preferences.h>
|
||||
#include <BLEDevice.h>
|
||||
#include <BLEServer.h>
|
||||
#include <BLEUtils.h>
|
||||
#include <BLE2902.h>
|
||||
#include "DW1000Ranging.h"
|
||||
|
||||
#include <Adafruit_GFX.h>
|
||||
@ -78,25 +102,24 @@ Adafruit_SSD1306 display(128, 64, &Wire, -1);
|
||||
#define MPU6050_ACCEL_CFG 0x1C
|
||||
#define MPU6050_GYRO_CFG 0x1B
|
||||
#define MPU6050_ACCEL_XOUT 0x3B
|
||||
#define MPU6050_INT_CFG 0x37 /* INT pin config */
|
||||
#define MPU6050_INT_EN 0x38 /* Interrupt enable */
|
||||
#define MPU6050_MOT_THR 0x1F /* Motion threshold */
|
||||
#define MPU6050_MOT_DUR 0x20 /* Motion duration */
|
||||
#define MPU6050_INT_CFG 0x37
|
||||
#define MPU6050_INT_EN 0x38
|
||||
#define MPU6050_MOT_THR 0x1F
|
||||
#define MPU6050_MOT_DUR 0x20
|
||||
|
||||
static bool g_imu_ok = false;
|
||||
|
||||
struct ImuData {
|
||||
float ax, ay, az; /* g (±8g range) */
|
||||
float gx, gy, gz; /* deg/s (±500 dps range) */
|
||||
float accel_mag; /* total acceleration magnitude */
|
||||
float ax, ay, az;
|
||||
float gx, gy, gz;
|
||||
float accel_mag;
|
||||
};
|
||||
|
||||
static ImuData g_imu = {};
|
||||
|
||||
/* Fall detection */
|
||||
#define FALL_ACCEL_THRESHOLD 2.5f /* >2.5g = possible fall/impact */
|
||||
#define FALL_FREEFALL_LOW 0.3f /* <0.3g = freefall phase */
|
||||
#define FALL_COOLDOWN_MS 5000 /* Don't re-trigger for 5s */
|
||||
#define FALL_ACCEL_THRESHOLD 2.5f
|
||||
#define FALL_FREEFALL_LOW 0.3f
|
||||
#define FALL_COOLDOWN_MS 5000
|
||||
|
||||
static bool g_fall_detected = false;
|
||||
static uint32_t g_fall_last_ms = 0;
|
||||
@ -119,24 +142,16 @@ static uint8_t mpu6050_read_byte(uint8_t reg) {
|
||||
}
|
||||
|
||||
static bool mpu6050_init(void) {
|
||||
/* Check WHO_AM_I (should be 0x68 for MPU6050, 0x72 for MPU6050C) */
|
||||
uint8_t who = mpu6050_read_byte(MPU6050_WHO_AM_I);
|
||||
if (who != 0x68 && who != 0x72 && who != 0x70) {
|
||||
Serial.printf("[imu] WHO_AM_I=0x%02X — not MPU6050\r\n", who);
|
||||
return false;
|
||||
}
|
||||
Serial.printf("[imu] MPU6050 found, WHO_AM_I=0x%02X\r\n", who);
|
||||
|
||||
/* Wake up (clear sleep bit), use gyro X PLL as clock */
|
||||
mpu6050_write(MPU6050_PWR_MGMT_1, 0x01);
|
||||
delay(10);
|
||||
|
||||
/* Accel range: ±8g (sensitivity = 4096 LSB/g) */
|
||||
mpu6050_write(MPU6050_ACCEL_CFG, 0x10);
|
||||
|
||||
/* Gyro range: ±500 dps (sensitivity = 65.5 LSB/°/s) */
|
||||
mpu6050_write(MPU6050_GYRO_CFG, 0x08);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -150,40 +165,26 @@ static void mpu6050_read(ImuData *d) {
|
||||
for (int i = 0; i < 7; i++) {
|
||||
raw[i] = (Wire.read() << 8) | Wire.read();
|
||||
}
|
||||
|
||||
/* ±8g → 4096 LSB/g */
|
||||
d->ax = raw[0] / 4096.0f;
|
||||
d->ay = raw[1] / 4096.0f;
|
||||
d->az = raw[2] / 4096.0f;
|
||||
/* raw[3] = temperature, skip */
|
||||
/* ±500 dps → 65.5 LSB/°/s */
|
||||
d->gx = raw[4] / 65.5f;
|
||||
d->gy = raw[5] / 65.5f;
|
||||
d->gz = raw[6] / 65.5f;
|
||||
|
||||
d->accel_mag = sqrtf(d->ax * d->ax + d->ay * d->ay + d->az * d->az);
|
||||
}
|
||||
|
||||
/* Fall detection state machine:
|
||||
* 1. Freefall phase: accel_mag < 0.3g for >50ms
|
||||
* 2. Impact phase: accel_mag > 2.5g within 500ms of freefall
|
||||
* Either a sudden impact alone (>3g) also triggers.
|
||||
*/
|
||||
static void fall_detect(const ImuData *d) {
|
||||
uint32_t now = millis();
|
||||
|
||||
/* Cooldown */
|
||||
if (g_fall_detected && (now - g_fall_last_ms) < FALL_COOLDOWN_MS) return;
|
||||
g_fall_detected = false;
|
||||
|
||||
/* Check freefall */
|
||||
if (d->accel_mag < FALL_FREEFALL_LOW) {
|
||||
if (!g_freefall_phase) {
|
||||
g_freefall_phase = true;
|
||||
g_freefall_start = now;
|
||||
}
|
||||
} else {
|
||||
/* Check for impact after freefall */
|
||||
if (g_freefall_phase && (now - g_freefall_start) > 50) {
|
||||
if (d->accel_mag > FALL_ACCEL_THRESHOLD &&
|
||||
(now - g_freefall_start) < 500) {
|
||||
@ -196,7 +197,6 @@ static void fall_detect(const ImuData *d) {
|
||||
g_freefall_phase = false;
|
||||
}
|
||||
|
||||
/* Sudden high-g impact without freefall (e.g., side collision) */
|
||||
if (!g_fall_detected && d->accel_mag > 3.5f) {
|
||||
g_fall_detected = true;
|
||||
g_fall_last_ms = now;
|
||||
@ -204,6 +204,299 @@ static void fall_detect(const ImuData *d) {
|
||||
}
|
||||
}
|
||||
|
||||
/* ── BLE Configuration ───────────────────────────────────────────
|
||||
*
|
||||
* GATT layout:
|
||||
* Service 12345678-1234-5678-1234-56789abcdef0
|
||||
* Config 12345678-1234-5678-1234-56789abcdef1 [READ | WRITE]
|
||||
* Status 12345678-1234-5678-1234-56789abcdef2 [READ | NOTIFY]
|
||||
*
|
||||
* Config characteristic read returns:
|
||||
* {"sleep_timeout_s":30,"display_brightness":200,"tag_name":"UWB_TAG_1A2B",
|
||||
* "uwb_channel":5,"ranging_interval_ms":100,"battery_report":true}
|
||||
*
|
||||
* Config characteristic write accepts any valid JSON subset of those keys.
|
||||
* Status characteristic notifies "+OK" on success or "+ERR:<reason>" on fail.
|
||||
* All config persisted to NVS namespace "uwb_cfg".
|
||||
* ─────────────────────────────────────────────────────────────── */
|
||||
|
||||
#define BLE_SVC_UUID "12345678-1234-5678-1234-56789abcdef0"
|
||||
#define BLE_CFG_UUID "12345678-1234-5678-1234-56789abcdef1"
|
||||
#define BLE_STS_UUID "12345678-1234-5678-1234-56789abcdef2"
|
||||
|
||||
#define CFG_NVS_NS "uwb_cfg"
|
||||
#define CFG_DEFAULT_SLEEP_S 30
|
||||
#define CFG_DEFAULT_BRIGHTNESS 200
|
||||
#define CFG_DEFAULT_CHANNEL 5
|
||||
#define CFG_DEFAULT_RANGING_MS 100
|
||||
#define CFG_DEFAULT_BATTERY_REPORT true
|
||||
|
||||
struct UwbConfig {
|
||||
uint16_t sleep_timeout_s;
|
||||
uint8_t display_brightness;
|
||||
char tag_name[17]; /* max 16 chars + NUL */
|
||||
uint8_t uwb_channel;
|
||||
uint16_t ranging_interval_ms;
|
||||
bool battery_report;
|
||||
};
|
||||
|
||||
static UwbConfig g_cfg = {};
|
||||
static Preferences g_prefs;
|
||||
static BLECharacteristic *g_ble_cfg_char = nullptr;
|
||||
static BLECharacteristic *g_ble_sts_char = nullptr;
|
||||
static bool g_ble_connected = false;
|
||||
static char g_ble_device_name[20] = {};
|
||||
|
||||
/* ── NVS config persistence ──────────────────────────────────────── */
|
||||
|
||||
static void cfg_load(void) {
|
||||
g_prefs.begin(CFG_NVS_NS, true); /* read-only */
|
||||
g_cfg.sleep_timeout_s = g_prefs.getUShort("sleep_s", CFG_DEFAULT_SLEEP_S);
|
||||
g_cfg.display_brightness= g_prefs.getUChar ("brightness", CFG_DEFAULT_BRIGHTNESS);
|
||||
g_cfg.uwb_channel = g_prefs.getUChar ("channel", CFG_DEFAULT_CHANNEL);
|
||||
g_cfg.ranging_interval_ms = g_prefs.getUShort("rng_ms", CFG_DEFAULT_RANGING_MS);
|
||||
g_cfg.battery_report = g_prefs.getBool ("bat_rpt", CFG_DEFAULT_BATTERY_REPORT);
|
||||
String nm = g_prefs.getString("tag_name", "");
|
||||
if (nm.length() > 0 && nm.length() <= 16) {
|
||||
strncpy(g_cfg.tag_name, nm.c_str(), sizeof(g_cfg.tag_name) - 1);
|
||||
g_cfg.tag_name[sizeof(g_cfg.tag_name) - 1] = '\0';
|
||||
} else {
|
||||
strncpy(g_cfg.tag_name, g_ble_device_name, sizeof(g_cfg.tag_name) - 1);
|
||||
g_cfg.tag_name[sizeof(g_cfg.tag_name) - 1] = '\0';
|
||||
}
|
||||
g_prefs.end();
|
||||
|
||||
/* Clamp values to valid ranges */
|
||||
if (g_cfg.sleep_timeout_s < 5) g_cfg.sleep_timeout_s = 5;
|
||||
if (g_cfg.sleep_timeout_s > 3600) g_cfg.sleep_timeout_s = 3600;
|
||||
if (g_cfg.uwb_channel < 1) g_cfg.uwb_channel = 1;
|
||||
if (g_cfg.uwb_channel > 7) g_cfg.uwb_channel = 7;
|
||||
if (g_cfg.ranging_interval_ms < 50) g_cfg.ranging_interval_ms = 50;
|
||||
if (g_cfg.ranging_interval_ms > 2000) g_cfg.ranging_interval_ms = 2000;
|
||||
|
||||
Serial.printf("[cfg] Loaded: sleep=%ds bright=%d name=%s ch=%d rng=%dms bat=%d\r\n",
|
||||
g_cfg.sleep_timeout_s, g_cfg.display_brightness,
|
||||
g_cfg.tag_name, g_cfg.uwb_channel,
|
||||
g_cfg.ranging_interval_ms, (int)g_cfg.battery_report);
|
||||
}
|
||||
|
||||
static void cfg_save(void) {
|
||||
g_prefs.begin(CFG_NVS_NS, false); /* read-write */
|
||||
g_prefs.putUShort("sleep_s", g_cfg.sleep_timeout_s);
|
||||
g_prefs.putUChar ("brightness", g_cfg.display_brightness);
|
||||
g_prefs.putUChar ("channel", g_cfg.uwb_channel);
|
||||
g_prefs.putUShort("rng_ms", g_cfg.ranging_interval_ms);
|
||||
g_prefs.putBool ("bat_rpt", g_cfg.battery_report);
|
||||
g_prefs.putString("tag_name", g_cfg.tag_name);
|
||||
g_prefs.end();
|
||||
Serial.println("[cfg] Saved to NVS");
|
||||
}
|
||||
|
||||
/* Apply config values that take effect immediately */
|
||||
static void cfg_apply(void) {
|
||||
/* OLED brightness (contrast) — 0x00=dim, 0xFF=bright */
|
||||
display.ssd1306_command(SSD1306_SETCONTRAST);
|
||||
display.ssd1306_command(g_cfg.display_brightness);
|
||||
Serial.printf("[cfg] Applied brightness=%d\r\n", g_cfg.display_brightness);
|
||||
/* Note: uwb_channel and ranging_interval_ms apply at next boot */
|
||||
}
|
||||
|
||||
/* Build JSON string from current config — caller owns the buffer */
|
||||
static void cfg_to_json(char *buf, size_t len) {
|
||||
snprintf(buf, len,
|
||||
"{\"sleep_timeout_s\":%d,"
|
||||
"\"display_brightness\":%d,"
|
||||
"\"tag_name\":\"%s\","
|
||||
"\"uwb_channel\":%d,"
|
||||
"\"ranging_interval_ms\":%d,"
|
||||
"\"battery_report\":%s}",
|
||||
g_cfg.sleep_timeout_s,
|
||||
g_cfg.display_brightness,
|
||||
g_cfg.tag_name,
|
||||
g_cfg.uwb_channel,
|
||||
g_cfg.ranging_interval_ms,
|
||||
g_cfg.battery_report ? "true" : "false");
|
||||
}
|
||||
|
||||
/* ── Minimal JSON field extractor ────────────────────────────────
|
||||
* Finds "key":value in a JSON string.
|
||||
* Returns pointer to value start in src, or nullptr if not found.
|
||||
* For strings returns content between quotes.
|
||||
* For numbers/bools returns pointer to first digit/letter.
|
||||
* ─────────────────────────────────────────────────────────────── */
|
||||
|
||||
/* Find "key": in json, return pointer just past the colon+space */
|
||||
static const char *json_find_key(const char *json, const char *key) {
|
||||
char needle[32];
|
||||
snprintf(needle, sizeof(needle), "\"%s\"", key);
|
||||
const char *p = strstr(json, needle);
|
||||
if (!p) return nullptr;
|
||||
p += strlen(needle);
|
||||
while (*p == ' ' || *p == ':') p++;
|
||||
return p;
|
||||
}
|
||||
|
||||
static bool json_get_int(const char *json, const char *key, int *out) {
|
||||
const char *p = json_find_key(json, key);
|
||||
if (!p || *p == '\0') return false;
|
||||
char *end;
|
||||
long v = strtol(p, &end, 10);
|
||||
if (end == p) return false;
|
||||
*out = (int)v;
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool json_get_bool(const char *json, const char *key, bool *out) {
|
||||
const char *p = json_find_key(json, key);
|
||||
if (!p) return false;
|
||||
if (strncmp(p, "true", 4) == 0) { *out = true; return true; }
|
||||
if (strncmp(p, "false", 5) == 0) { *out = false; return true; }
|
||||
return false;
|
||||
}
|
||||
|
||||
/* Extract string value into buf[len], returns false if not found or too long */
|
||||
static bool json_get_str(const char *json, const char *key, char *buf, size_t len) {
|
||||
const char *p = json_find_key(json, key);
|
||||
if (!p || *p != '"') return false;
|
||||
p++; /* skip opening quote */
|
||||
const char *end = strchr(p, '"');
|
||||
if (!end) return false;
|
||||
size_t slen = end - p;
|
||||
if (slen >= len) return false;
|
||||
memcpy(buf, p, slen);
|
||||
buf[slen] = '\0';
|
||||
return true;
|
||||
}
|
||||
|
||||
/* Parse incoming JSON, update g_cfg, return error string or nullptr */
|
||||
static const char *cfg_parse_json(const char *json) {
|
||||
int v_int;
|
||||
bool v_bool;
|
||||
char v_str[17];
|
||||
|
||||
if (json_get_int(json, "sleep_timeout_s", &v_int)) {
|
||||
if (v_int < 5 || v_int > 3600) return "sleep_timeout_s out of range [5..3600]";
|
||||
g_cfg.sleep_timeout_s = (uint16_t)v_int;
|
||||
}
|
||||
if (json_get_int(json, "display_brightness", &v_int)) {
|
||||
if (v_int < 0 || v_int > 255) return "display_brightness out of range [0..255]";
|
||||
g_cfg.display_brightness = (uint8_t)v_int;
|
||||
}
|
||||
if (json_get_str(json, "tag_name", v_str, sizeof(v_str))) {
|
||||
strncpy(g_cfg.tag_name, v_str, sizeof(g_cfg.tag_name) - 1);
|
||||
g_cfg.tag_name[sizeof(g_cfg.tag_name) - 1] = '\0';
|
||||
}
|
||||
if (json_get_int(json, "uwb_channel", &v_int)) {
|
||||
if (v_int < 1 || v_int > 7) return "uwb_channel out of range [1..7]";
|
||||
g_cfg.uwb_channel = (uint8_t)v_int;
|
||||
}
|
||||
if (json_get_int(json, "ranging_interval_ms", &v_int)) {
|
||||
if (v_int < 50 || v_int > 2000) return "ranging_interval_ms out of range [50..2000]";
|
||||
g_cfg.ranging_interval_ms = (uint16_t)v_int;
|
||||
}
|
||||
if (json_get_bool(json, "battery_report", &v_bool)) {
|
||||
g_cfg.battery_report = v_bool;
|
||||
}
|
||||
return nullptr; /* success */
|
||||
}
|
||||
|
||||
/* ── BLE server callbacks ─────────────────────────────────────── */
|
||||
|
||||
class BleServerCb : public BLEServerCallbacks {
|
||||
void onConnect(BLEServer *) override {
|
||||
g_ble_connected = true;
|
||||
Serial.println("[ble] Client connected");
|
||||
}
|
||||
void onDisconnect(BLEServer *server) override {
|
||||
g_ble_connected = false;
|
||||
Serial.println("[ble] Client disconnected — restarting advertising");
|
||||
server->startAdvertising();
|
||||
}
|
||||
};
|
||||
|
||||
class BleCfgCb : public BLECharacteristicCallbacks {
|
||||
/* READ: return current config as JSON */
|
||||
void onRead(BLECharacteristic *ch) override {
|
||||
char buf[256];
|
||||
cfg_to_json(buf, sizeof(buf));
|
||||
ch->setValue(buf);
|
||||
Serial.printf("[ble] Config read: %s\r\n", buf);
|
||||
}
|
||||
|
||||
/* WRITE: parse JSON, apply, save, notify status */
|
||||
void onWrite(BLECharacteristic *ch) override {
|
||||
String val = ch->getValue();
|
||||
if (val.length() == 0) {
|
||||
ble_notify_status("+ERR:empty payload");
|
||||
return;
|
||||
}
|
||||
|
||||
Serial.printf("[ble] Config write: %s\r\n", val.c_str());
|
||||
const char *err = cfg_parse_json(val.c_str()); // String::c_str() is valid
|
||||
if (err) {
|
||||
char msg[64];
|
||||
snprintf(msg, sizeof(msg), "+ERR:%s", err);
|
||||
ble_notify_status(msg);
|
||||
Serial.printf("[ble] Config error: %s\r\n", err);
|
||||
return;
|
||||
}
|
||||
|
||||
cfg_save();
|
||||
cfg_apply();
|
||||
ble_notify_status("+OK");
|
||||
Serial.println("[ble] Config applied OK");
|
||||
}
|
||||
|
||||
static void ble_notify_status(const char *msg) {
|
||||
if (g_ble_sts_char) {
|
||||
g_ble_sts_char->setValue(msg);
|
||||
g_ble_sts_char->notify();
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
/* ── BLE init ─────────────────────────────────────────────────── */
|
||||
|
||||
static void ble_init(void) {
|
||||
BLEDevice::init(g_ble_device_name);
|
||||
BLEServer *server = BLEDevice::createServer();
|
||||
server->setCallbacks(new BleServerCb());
|
||||
|
||||
BLEService *svc = server->createService(BLE_SVC_UUID);
|
||||
|
||||
/* Config characteristic — readable and writable */
|
||||
g_ble_cfg_char = svc->createCharacteristic(
|
||||
BLE_CFG_UUID,
|
||||
BLECharacteristic::PROPERTY_READ |
|
||||
BLECharacteristic::PROPERTY_WRITE |
|
||||
BLECharacteristic::PROPERTY_WRITE_NR
|
||||
);
|
||||
g_ble_cfg_char->setCallbacks(new BleCfgCb());
|
||||
|
||||
/* Status characteristic — readable and notifiable */
|
||||
g_ble_sts_char = svc->createCharacteristic(
|
||||
BLE_STS_UUID,
|
||||
BLECharacteristic::PROPERTY_READ |
|
||||
BLECharacteristic::PROPERTY_NOTIFY
|
||||
);
|
||||
g_ble_sts_char->addDescriptor(new BLE2902());
|
||||
g_ble_sts_char->setValue("+OK");
|
||||
|
||||
svc->start();
|
||||
|
||||
BLEAdvertising *adv = BLEDevice::getAdvertising();
|
||||
adv->addServiceUUID(BLE_SVC_UUID);
|
||||
adv->setScanResponse(true);
|
||||
adv->setMinPreferred(0x06);
|
||||
adv->setMinPreferred(0x12);
|
||||
BLEDevice::startAdvertising();
|
||||
|
||||
Serial.printf("[ble] Advertising as \"%s\"\r\n", g_ble_device_name);
|
||||
Serial.printf("[ble] Service UUID: %s\r\n", BLE_SVC_UUID);
|
||||
Serial.printf("[ble] Config UUID: %s\r\n", BLE_CFG_UUID);
|
||||
Serial.printf("[ble] Status UUID: %s\r\n", BLE_STS_UUID);
|
||||
}
|
||||
|
||||
/* ── ESP-NOW ────────────────────────────────────────────────────── */
|
||||
|
||||
#define ESPNOW_MAGIC_0 0x5B
|
||||
@ -232,38 +525,34 @@ struct EspNowPacket {
|
||||
uint8_t _pad;
|
||||
};
|
||||
|
||||
/* Extended IMU packet — fits in 20 bytes too */
|
||||
struct EspNowImuPacket {
|
||||
uint8_t magic[2]; /* 0x5B 0x01 */
|
||||
uint8_t magic[2];
|
||||
uint8_t tag_id;
|
||||
uint8_t msg_type; /* MSG_IMU or MSG_FALL */
|
||||
int16_t ax_mg; /* accel X in milli-g */
|
||||
int16_t ay_mg; /* accel Y in milli-g */
|
||||
int16_t az_mg; /* accel Z in milli-g */
|
||||
int16_t gx_dps10; /* gyro X in 0.1 deg/s */
|
||||
int16_t gy_dps10; /* gyro Y */
|
||||
int16_t gz_dps10; /* gyro Z */
|
||||
uint8_t accel_mag_10; /* total accel in 0.1g (max 25.5g) */
|
||||
uint8_t msg_type;
|
||||
int16_t ax_mg;
|
||||
int16_t ay_mg;
|
||||
int16_t az_mg;
|
||||
int16_t gx_dps10;
|
||||
int16_t gy_dps10;
|
||||
int16_t gz_dps10;
|
||||
uint8_t accel_mag_10;
|
||||
uint8_t seq_num;
|
||||
uint16_t timestamp_s10; /* uptime in 0.1s (wraps at ~109 min) */
|
||||
uint16_t timestamp_s10;
|
||||
};
|
||||
#pragma pack(pop)
|
||||
|
||||
/* ── Anchor tracking (declared early for ESP-NOW RX callback) ──── */
|
||||
/* ── Anchor tracking ────────────────────────────────────────────── */
|
||||
|
||||
static int32_t g_anchor_range_mm[NUM_ANCHORS];
|
||||
static float g_anchor_rssi[NUM_ANCHORS];
|
||||
static uint32_t g_anchor_last_ok[NUM_ANCHORS];
|
||||
|
||||
/* ── ESP-NOW RX: receive range data from anchors ────────────────── */
|
||||
|
||||
static void IRAM_ATTR espnow_rx_cb(const esp_now_recv_info_t *info,
|
||||
const uint8_t *data, int len) {
|
||||
if (len < (int)sizeof(EspNowPacket)) return;
|
||||
const EspNowPacket *pkt = (const EspNowPacket *)data;
|
||||
if (pkt->magic[0] != ESPNOW_MAGIC_0 || pkt->magic[1] != ESPNOW_MAGIC_1) return;
|
||||
|
||||
/* Only process anchor range broadcasts (anchor_id 0 or 1) */
|
||||
if (pkt->msg_type == MSG_RANGE && pkt->anchor_id < NUM_ANCHORS) {
|
||||
int idx = pkt->anchor_id;
|
||||
g_anchor_range_mm[idx] = pkt->range_mm;
|
||||
@ -272,8 +561,6 @@ static void IRAM_ATTR espnow_rx_cb(const esp_now_recv_info_t *info,
|
||||
}
|
||||
}
|
||||
|
||||
/* ── ESP-NOW TX ─────────────────────────────────────────────────── */
|
||||
|
||||
static void espnow_send(uint8_t msg_type, uint8_t anchor_id,
|
||||
int32_t range_mm, float rssi) {
|
||||
EspNowPacket pkt = {};
|
||||
@ -285,7 +572,8 @@ static void espnow_send(uint8_t msg_type, uint8_t anchor_id,
|
||||
pkt.range_mm = range_mm;
|
||||
pkt.rssi_dbm = rssi;
|
||||
pkt.timestamp_ms = millis();
|
||||
pkt.battery_pct = 0xFF; /* TODO: battery ADC */
|
||||
/* battery_pct: include voltage reading if battery_report enabled */
|
||||
pkt.battery_pct = g_cfg.battery_report ? 0xFE : 0xFF; /* 0xFE=reporting, 0xFF=disabled */
|
||||
pkt.flags = 0;
|
||||
pkt.seq_num = g_seq++;
|
||||
esp_now_send(broadcast_mac, (uint8_t *)&pkt, sizeof(pkt));
|
||||
@ -309,7 +597,7 @@ static void espnow_send_imu(const ImuData *d, uint8_t msg_type) {
|
||||
esp_now_send(broadcast_mac, (uint8_t *)&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
/* ── Anchor tracking (DW1000 address mapping) ──────────────────── */
|
||||
/* ── DW1000 address mapping ─────────────────────────────────────── */
|
||||
|
||||
static uint16_t g_anchor_addrs[NUM_ANCHORS] = {0, 0};
|
||||
static int g_num_known_anchors = 0;
|
||||
@ -330,17 +618,11 @@ static int anchor_index(uint16_t addr) {
|
||||
/* ── E-Stop ─────────────────────────────────────────────────────── */
|
||||
|
||||
static bool g_estop_active = false;
|
||||
static uint32_t g_estop_last_tx = 0;
|
||||
|
||||
static void estop_check(void) {
|
||||
/* GPIO 0 (BOOT) floats LOW on Makerfabs display board — disabled.
|
||||
* Fall detection acts as automatic e-stop instead. */
|
||||
|
||||
/* Auto e-stop on fall detection */
|
||||
if (g_fall_detected && !g_estop_active) {
|
||||
g_estop_active = true;
|
||||
Serial.println("+ESTOP:FALL");
|
||||
/* Send immediate e-stop */
|
||||
for (int i = 0; i < 3; i++) {
|
||||
EspNowPacket pkt = {};
|
||||
pkt.magic[0] = ESPNOW_MAGIC_0;
|
||||
@ -353,11 +635,9 @@ static void estop_check(void) {
|
||||
esp_now_send(broadcast_mac, (uint8_t *)&pkt, sizeof(pkt));
|
||||
delay(5);
|
||||
}
|
||||
/* Also send fall event */
|
||||
espnow_send_imu(&g_imu, MSG_FALL);
|
||||
}
|
||||
|
||||
/* Auto-clear after cooldown */
|
||||
if (g_estop_active && !g_fall_detected) {
|
||||
g_estop_active = false;
|
||||
Serial.println("+ESTOP:CLEAR");
|
||||
@ -400,16 +680,22 @@ static void display_update(void) {
|
||||
return;
|
||||
}
|
||||
|
||||
/* Show BLE info briefly when connected */
|
||||
if (g_ble_connected) {
|
||||
display.setTextSize(1);
|
||||
display.setTextColor(SSD1306_WHITE);
|
||||
display.setCursor(88, 0);
|
||||
display.print(F("[BLE]"));
|
||||
}
|
||||
|
||||
uint32_t now = millis();
|
||||
|
||||
/* Find closest anchor */
|
||||
int32_t min_range = INT32_MAX;
|
||||
for (int i = 0; i < NUM_ANCHORS; i++) {
|
||||
if (g_anchor_range_mm[i] > 0 && g_anchor_range_mm[i] < min_range)
|
||||
min_range = g_anchor_range_mm[i];
|
||||
}
|
||||
|
||||
/* Line 1: Big distance */
|
||||
display.setTextSize(3);
|
||||
display.setTextColor(SSD1306_WHITE);
|
||||
display.setCursor(0, 0);
|
||||
@ -423,7 +709,6 @@ static void display_update(void) {
|
||||
display.println(F("---"));
|
||||
}
|
||||
|
||||
/* Line 2: Both anchor ranges */
|
||||
display.setTextSize(1);
|
||||
display.setCursor(0, 28);
|
||||
for (int i = 0; i < NUM_ANCHORS; i++) {
|
||||
@ -434,23 +719,16 @@ static void display_update(void) {
|
||||
}
|
||||
}
|
||||
|
||||
/* Line 3: IMU data or link status */
|
||||
display.setCursor(0, 40);
|
||||
if (g_imu_ok) {
|
||||
/* Show accel magnitude + tilt indicator */
|
||||
display.printf("%.1fg", g_imu.accel_mag);
|
||||
|
||||
/* Mini tilt bar (visual lean indicator) */
|
||||
int tilt_x = constrain((int)(g_imu.ax * 20.0f), -30, 30);
|
||||
int bar_center = 80;
|
||||
display.drawFastHLine(bar_center - 30, 44, 60, SSD1306_WHITE);
|
||||
display.fillRect(bar_center + tilt_x - 2, 42, 4, 4, SSD1306_WHITE);
|
||||
|
||||
/* Gyro Z (yaw rate) */
|
||||
display.setCursor(100, 40);
|
||||
display.printf("%+.0f", g_imu.gz);
|
||||
} else {
|
||||
/* No IMU — show link status */
|
||||
bool any_linked = false;
|
||||
for (int i = 0; i < NUM_ANCHORS; i++) {
|
||||
if (g_anchor_last_ok[i] > 0 && (now - g_anchor_last_ok[i]) < 2000) {
|
||||
@ -461,7 +739,6 @@ static void display_update(void) {
|
||||
display.println(any_linked ? F("LINKED") : F("SEARCHING..."));
|
||||
}
|
||||
|
||||
/* Line 4: Uptime + seq + IMU status */
|
||||
display.setCursor(0, 54);
|
||||
uint32_t secs = now / 1000;
|
||||
display.printf("UP%02d:%02d p:%d", secs / 60, secs % 60, g_seq);
|
||||
@ -521,6 +798,12 @@ void setup(void) {
|
||||
|
||||
Serial.printf("\r\n[uwb_tag] tag_id=0x%02X starting\r\n", TAG_ID);
|
||||
|
||||
/* Build BLE device name from WiFi MAC last 4 hex digits */
|
||||
uint8_t mac[6];
|
||||
WiFi.macAddress(mac);
|
||||
snprintf(g_ble_device_name, sizeof(g_ble_device_name),
|
||||
"UWB_TAG_%02X%02X", mac[4], mac[5]);
|
||||
|
||||
/* I2C bus (shared: OLED @0x3C + MPU6050 @0x68) */
|
||||
Wire.begin(PIN_SDA, PIN_SCL);
|
||||
|
||||
@ -537,24 +820,24 @@ void setup(void) {
|
||||
display.setCursor(0, 20);
|
||||
display.printf("Tag 0x%02X", TAG_ID);
|
||||
display.setCursor(0, 35);
|
||||
display.println(F("DW1000 200m + ESP-NOW"));
|
||||
display.println(F("DW1000 200m + BLE cfg"));
|
||||
display.setCursor(0, 50);
|
||||
display.println(F("Init IMU..."));
|
||||
display.printf("BLE: %s", g_ble_device_name);
|
||||
display.display();
|
||||
}
|
||||
|
||||
/* Load config from NVS (requires g_ble_device_name for default tag_name) */
|
||||
cfg_load();
|
||||
|
||||
/* Apply display brightness from config */
|
||||
cfg_apply();
|
||||
|
||||
/* MPU6050 */
|
||||
g_imu_ok = mpu6050_init();
|
||||
if (g_imu_ok) {
|
||||
Serial.println("[uwb_tag] MPU6050 init OK (±8g, ±500dps)");
|
||||
display.setCursor(0, 50);
|
||||
display.println(F("IMU OK! Searching..."));
|
||||
display.display();
|
||||
} else {
|
||||
Serial.println("[uwb_tag] MPU6050 not found — running without IMU");
|
||||
display.setCursor(0, 50);
|
||||
display.println(F("No IMU. Searching..."));
|
||||
display.display();
|
||||
}
|
||||
|
||||
/* ESP-NOW */
|
||||
@ -573,17 +856,18 @@ void setup(void) {
|
||||
Serial.println("[uwb_tag] WARN: ESP-NOW failed");
|
||||
}
|
||||
|
||||
/* BLE */
|
||||
ble_init();
|
||||
|
||||
/* DW1000 */
|
||||
SPI.begin(PIN_SCK, PIN_MISO, PIN_MOSI);
|
||||
DW1000Ranging.initCommunication(PIN_RST, PIN_CS, PIN_IRQ);
|
||||
|
||||
DW1000Ranging.attachNewRange(newRange);
|
||||
DW1000Ranging.attachNewDevice(newDevice);
|
||||
DW1000Ranging.attachInactiveDevice(inactiveDevice);
|
||||
|
||||
DW1000Ranging.startAsTag(TAG_ADDR, DW1000.MODE_LONGDATA_RANGE_LOWPOWER);
|
||||
|
||||
/* Init state */
|
||||
/* Init anchor state */
|
||||
for (int i = 0; i < NUM_ANCHORS; i++) {
|
||||
g_anchor_range_mm[i] = -1;
|
||||
g_anchor_rssi[i] = -100.0f;
|
||||
@ -591,7 +875,7 @@ void setup(void) {
|
||||
}
|
||||
|
||||
Serial.println("[uwb_tag] DW1000 ready MODE_LONGDATA_RANGE_LOWPOWER");
|
||||
Serial.println("[uwb_tag] Ranging + display + ESP-NOW active");
|
||||
Serial.println("[uwb_tag] Ranging + display + ESP-NOW + BLE active");
|
||||
}
|
||||
|
||||
/* ── Loop ───────────────────────────────────────────────────────── */
|
||||
@ -600,29 +884,27 @@ static uint32_t g_last_hb = 0;
|
||||
static uint32_t g_last_imu = 0;
|
||||
|
||||
void loop(void) {
|
||||
/* IMU read at ~50 Hz (fast enough for fall detection) */
|
||||
/* IMU read at ~50 Hz */
|
||||
if (g_imu_ok && (millis() - g_last_imu >= 20)) {
|
||||
mpu6050_read(&g_imu);
|
||||
fall_detect(&g_imu);
|
||||
g_last_imu = millis();
|
||||
}
|
||||
|
||||
/* E-stop (now triggered by fall detection) */
|
||||
estop_check();
|
||||
|
||||
/* Heartbeat every 1s — includes IMU data if available */
|
||||
/* Heartbeat every 1s */
|
||||
if (millis() - g_last_hb >= 1000) {
|
||||
espnow_send(MSG_HEARTBEAT, 0xFF, 0, 0.0f);
|
||||
g_last_hb = millis();
|
||||
}
|
||||
|
||||
/* IMU broadcast at 10 Hz via ESP-NOW */
|
||||
/* IMU broadcast at 10 Hz */
|
||||
static uint32_t s_last_imu_tx = 0;
|
||||
if (g_imu_ok && (millis() - s_last_imu_tx >= 100)) {
|
||||
espnow_send_imu(&g_imu, MSG_IMU);
|
||||
s_last_imu_tx = millis();
|
||||
|
||||
/* Serial output for debugging / ROS */
|
||||
Serial.printf("+IMU:%.2f,%.2f,%.2f,%.1f,%.1f,%.1f,%.2f\r\n",
|
||||
g_imu.ax, g_imu.ay, g_imu.az,
|
||||
g_imu.gx, g_imu.gy, g_imu.gz,
|
||||
@ -630,9 +912,12 @@ void loop(void) {
|
||||
s_last_imu_tx = millis();
|
||||
}
|
||||
|
||||
/* DW1000 ranging */
|
||||
if (!g_estop_active) {
|
||||
/* DW1000 ranging — throttled by configured interval */
|
||||
static uint32_t s_last_range_tick = 0;
|
||||
if (!g_estop_active &&
|
||||
(millis() - s_last_range_tick >= g_cfg.ranging_interval_ms)) {
|
||||
DW1000Ranging.loop();
|
||||
s_last_range_tick = millis();
|
||||
}
|
||||
|
||||
/* Display at 5 Hz */
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user