seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests 5 Actions Packages Projects Releases Wiki Activity

feat: UWB tag BLE configuration interface (Issue #690)

Browse Source 
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 2026-03-18 10:32:10 -04:00
parent cf0a83e78e
commit 8ee9b4cca9
2 changed files with 380 additions and 132 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
esp32/uwb_tag/platformio.ini
View File

@ -14,6 +14,7 @@ lib_extra_dirs = ../../lib
lib_deps = lib_deps =
adafruit/Adafruit SSD1306@^2.5.7 adafruit/Adafruit SSD1306@^2.5.7
adafruit/Adafruit GFX Library@^1.11.5 adafruit/Adafruit GFX Library@^1.11.5
board_build.partitions = huge_app.csv
build_flags = build_flags =
-DCORE_DEBUG_LEVEL=0 -DCORE_DEBUG_LEVEL=0
-DTAG_ID=0x01 -DTAG_ID=0x01

511
esp32/uwb_tag/src/main.cpp
View File

@ -29,15 +29,34 @@
* LED: GPIO 2 * LED: GPIO 2
* *
* Power management (Issue #689): * Power management (Issue #689):
* OLED auto-off after 30s inactivity → saves ~25mA * OLED auto-off after sleep_timeout_s inactivity → saves ~25mA
* DW1000 deep sleep after 5min idle → saves ~155mA (160mA→3.5μA) * DW1000 deep sleep after 5min idle → saves ~155mA (160mA→3.5μA)
* Periodic 10s wake window to reacquire anchors * Periodic 5s wake window to reacquire anchors every 30s
* ESP32 deep sleep on GPIO0 hold 3s → saves ~240mA total * ESP32 deep sleep on GPIO0 hold 3s → saves ~240mA total
* Wake on GPIO0 button press * Wake on GPIO0 button press
* Active: ~250mA Sleep: <5mA (50x reduction target) * Active: ~250mA Sleep: <5mA (50x reduction target)
* *
* NOTE: GPIO0 requires hardware pull-up (10kΩ to 3.3V) for reliable * NOTE: GPIO0 requires hardware pull-up (10kΩ to 3.3V) for reliable
* button detection. Internal pullup is marginal on some boards. * button detection. Internal pullup is marginal on some boards.
*
* 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> #include <Arduino.h>
@ -48,6 +67,11 @@
#include <esp_wifi.h> #include <esp_wifi.h>
#include <esp_sleep.h> #include <esp_sleep.h>
#include <driver/gpio.h> #include <driver/gpio.h>
#include <Preferences.h>
#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEUtils.h>
#include <BLE2902.h>
#include "DW1000Ranging.h" #include "DW1000Ranging.h"
#include <Adafruit_GFX.h> #include <Adafruit_GFX.h>
@ -91,25 +115,24 @@ Adafruit_SSD1306 display(128, 64, &Wire, -1);
#define MPU6050_ACCEL_CFG 0x1C #define MPU6050_ACCEL_CFG 0x1C
#define MPU6050_GYRO_CFG 0x1B #define MPU6050_GYRO_CFG 0x1B
#define MPU6050_ACCEL_XOUT 0x3B #define MPU6050_ACCEL_XOUT 0x3B
#define MPU6050_INT_CFG 0x37 /* INT pin config */ #define MPU6050_INT_CFG 0x37
#define MPU6050_INT_EN 0x38 /* Interrupt enable */ #define MPU6050_INT_EN 0x38
#define MPU6050_MOT_THR 0x1F /* Motion threshold */ #define MPU6050_MOT_THR 0x1F
#define MPU6050_MOT_DUR 0x20 /* Motion duration */ #define MPU6050_MOT_DUR 0x20
static bool g_imu_ok = false; static bool g_imu_ok = false;
struct ImuData { struct ImuData {
float ax, ay, az; /* g (±8g range) */ float ax, ay, az;
float gx, gy, gz; /* deg/s (±500 dps range) */ float gx, gy, gz;
float accel_mag; /* total acceleration magnitude */ float accel_mag;
}; };
static ImuData g_imu = {}; static ImuData g_imu = {};
/* Fall detection */ #define FALL_ACCEL_THRESHOLD 2.5f
#define FALL_ACCEL_THRESHOLD 2.5f /* >2.5g = possible fall/impact */ #define FALL_FREEFALL_LOW 0.3f
#define FALL_FREEFALL_LOW 0.3f /* <0.3g = freefall phase */ #define FALL_COOLDOWN_MS 5000
#define FALL_COOLDOWN_MS 5000 /* Don't re-trigger for 5s */
static bool g_fall_detected = false; static bool g_fall_detected = false;
static uint32_t g_fall_last_ms = 0; static uint32_t g_fall_last_ms = 0;
@ -132,24 +155,16 @@ static uint8_t mpu6050_read_byte(uint8_t reg) {
} }
static bool mpu6050_init(void) { 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); uint8_t who = mpu6050_read_byte(MPU6050_WHO_AM_I);
if (who != 0x68 && who != 0x72 && who != 0x70) { if (who != 0x68 && who != 0x72 && who != 0x70) {
Serial.printf("[imu] WHO_AM_I=0x%02X — not MPU6050\r\n", who); Serial.printf("[imu] WHO_AM_I=0x%02X — not MPU6050\r\n", who);
return false; return false;
} }
Serial.printf("[imu] MPU6050 found, WHO_AM_I=0x%02X\r\n", who); 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); mpu6050_write(MPU6050_PWR_MGMT_1, 0x01);
delay(10); delay(10);
/* Accel range: ±8g (sensitivity = 4096 LSB/g) */
mpu6050_write(MPU6050_ACCEL_CFG, 0x10); mpu6050_write(MPU6050_ACCEL_CFG, 0x10);
/* Gyro range: ±500 dps (sensitivity = 65.5 LSB/°/s) */
mpu6050_write(MPU6050_GYRO_CFG, 0x08); mpu6050_write(MPU6050_GYRO_CFG, 0x08);
return true; return true;
} }
@ -163,40 +178,26 @@ static void mpu6050_read(ImuData *d) {
for (int i = 0; i < 7; i++) { for (int i = 0; i < 7; i++) {
raw[i] = (Wire.read() << 8) | Wire.read(); raw[i] = (Wire.read() << 8) | Wire.read();
} }
/* ±8g → 4096 LSB/g */
d->ax = raw[0] / 4096.0f; d->ax = raw[0] / 4096.0f;
d->ay = raw[1] / 4096.0f; d->ay = raw[1] / 4096.0f;
d->az = raw[2] / 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->gx = raw[4] / 65.5f;
d->gy = raw[5] / 65.5f; d->gy = raw[5] / 65.5f;
d->gz = raw[6] / 65.5f; d->gz = raw[6] / 65.5f;
d->accel_mag = sqrtf(d->ax * d->ax + d->ay * d->ay + d->az * d->az); 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) { static void fall_detect(const ImuData *d) {
uint32_t now = millis(); uint32_t now = millis();
/* Cooldown */
if (g_fall_detected && (now - g_fall_last_ms) < FALL_COOLDOWN_MS) return; if (g_fall_detected && (now - g_fall_last_ms) < FALL_COOLDOWN_MS) return;
g_fall_detected = false; g_fall_detected = false;
/* Check freefall */
if (d->accel_mag < FALL_FREEFALL_LOW) { if (d->accel_mag < FALL_FREEFALL_LOW) {
if (!g_freefall_phase) { if (!g_freefall_phase) {
g_freefall_phase = true; g_freefall_phase = true;
g_freefall_start = now; g_freefall_start = now;
} }
} else { } else {
/* Check for impact after freefall */
if (g_freefall_phase && (now - g_freefall_start) > 50) { if (g_freefall_phase && (now - g_freefall_start) > 50) {
if (d->accel_mag > FALL_ACCEL_THRESHOLD && if (d->accel_mag > FALL_ACCEL_THRESHOLD &&
(now - g_freefall_start) < 500) { (now - g_freefall_start) < 500) {
@ -209,7 +210,6 @@ static void fall_detect(const ImuData *d) {
g_freefall_phase = false; g_freefall_phase = false;
} }
/* Sudden high-g impact without freefall (e.g., side collision) */
if (!g_fall_detected && d->accel_mag > 3.5f) { if (!g_fall_detected && d->accel_mag > 3.5f) {
g_fall_detected = true; g_fall_detected = true;
g_fall_last_ms = now; g_fall_last_ms = now;
@ -217,6 +217,277 @@ 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 full JSON config.
* Config characteristic write accepts any valid JSON subset.
* Status characteristic notifies "+OK" or "+ERR:<reason>" after write.
* 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];
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);
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();
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);
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");
}
static void cfg_apply(void) {
display.ssd1306_command(SSD1306_SETCONTRAST);
display.ssd1306_command(g_cfg.display_brightness);
Serial.printf("[cfg] Applied brightness=%d\r\n", g_cfg.display_brightness);
}
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 ──────────────────────────────── */
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;
}
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++;
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;
}
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;
}
/* ── 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 {
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);
}
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());
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);
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());
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 ────────────────────────────────────────────────────── */ /* ── ESP-NOW ────────────────────────────────────────────────────── */
#define ESPNOW_MAGIC_0 0x5B #define ESPNOW_MAGIC_0 0x5B
@ -245,38 +516,34 @@ struct EspNowPacket {
uint8_t _pad; uint8_t _pad;
}; };
/* Extended IMU packet — fits in 20 bytes too */
struct EspNowImuPacket { struct EspNowImuPacket {
uint8_t magic[2]; /* 0x5B 0x01 */ uint8_t magic[2];
uint8_t tag_id; uint8_t tag_id;
uint8_t msg_type; /* MSG_IMU or MSG_FALL */ uint8_t msg_type;
int16_t ax_mg; /* accel X in milli-g */ int16_t ax_mg;
int16_t ay_mg; /* accel Y in milli-g */ int16_t ay_mg;
int16_t az_mg; /* accel Z in milli-g */ int16_t az_mg;
int16_t gx_dps10; /* gyro X in 0.1 deg/s */ int16_t gx_dps10;
int16_t gy_dps10; /* gyro Y */ int16_t gy_dps10;
int16_t gz_dps10; /* gyro Z */ int16_t gz_dps10;
uint8_t accel_mag_10; /* total accel in 0.1g (max 25.5g) */ uint8_t accel_mag_10;
uint8_t seq_num; uint8_t seq_num;
uint16_t timestamp_s10; /* uptime in 0.1s (wraps at ~109 min) */ uint16_t timestamp_s10;
}; };
#pragma pack(pop) #pragma pack(pop)
/* ── Anchor tracking (declared early for ESP-NOW RX callback) ──── */ /* ── Anchor tracking ────────────────────────────────────────────── */
static int32_t g_anchor_range_mm[NUM_ANCHORS]; static int32_t g_anchor_range_mm[NUM_ANCHORS];
static float g_anchor_rssi[NUM_ANCHORS]; static float g_anchor_rssi[NUM_ANCHORS];
static uint32_t g_anchor_last_ok[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, static void IRAM_ATTR espnow_rx_cb(const esp_now_recv_info_t *info,
const uint8_t *data, int len) { const uint8_t *data, int len) {
if (len < (int)sizeof(EspNowPacket)) return; if (len < (int)sizeof(EspNowPacket)) return;
const EspNowPacket *pkt = (const EspNowPacket *)data; const EspNowPacket *pkt = (const EspNowPacket *)data;
if (pkt->magic[0] != ESPNOW_MAGIC_0 || pkt->magic[1] != ESPNOW_MAGIC_1) return; 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) { if (pkt->msg_type == MSG_RANGE && pkt->anchor_id < NUM_ANCHORS) {
int idx = pkt->anchor_id; int idx = pkt->anchor_id;
g_anchor_range_mm[idx] = pkt->range_mm; g_anchor_range_mm[idx] = pkt->range_mm;
@ -285,8 +552,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, static void espnow_send(uint8_t msg_type, uint8_t anchor_id,
int32_t range_mm, float rssi) { int32_t range_mm, float rssi) {
EspNowPacket pkt = {}; EspNowPacket pkt = {};
@ -298,7 +563,7 @@ static void espnow_send(uint8_t msg_type, uint8_t anchor_id,
pkt.range_mm = range_mm; pkt.range_mm = range_mm;
pkt.rssi_dbm = rssi; pkt.rssi_dbm = rssi;
pkt.timestamp_ms = millis(); pkt.timestamp_ms = millis();
pkt.battery_pct = 0xFF; /* TODO: battery ADC */ pkt.battery_pct = g_cfg.battery_report ? 0xFE : 0xFF;
pkt.flags = 0; pkt.flags = 0;
pkt.seq_num = g_seq++; pkt.seq_num = g_seq++;
esp_now_send(broadcast_mac, (uint8_t *)&pkt, sizeof(pkt)); esp_now_send(broadcast_mac, (uint8_t *)&pkt, sizeof(pkt));
@ -322,7 +587,7 @@ static void espnow_send_imu(const ImuData *d, uint8_t msg_type) {
esp_now_send(broadcast_mac, (uint8_t *)&pkt, sizeof(pkt)); 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 uint16_t g_anchor_addrs[NUM_ANCHORS] = {0, 0};
static int g_num_known_anchors = 0; static int g_num_known_anchors = 0;
@ -343,17 +608,11 @@ static int anchor_index(uint16_t addr) {
/* ── E-Stop ─────────────────────────────────────────────────────── */ /* ── E-Stop ─────────────────────────────────────────────────────── */
static bool g_estop_active = false; static bool g_estop_active = false;
static uint32_t g_estop_last_tx = 0;
static void estop_check(void) { 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) { if (g_fall_detected && !g_estop_active) {
g_estop_active = true; g_estop_active = true;
Serial.println("+ESTOP:FALL"); Serial.println("+ESTOP:FALL");
/* Send immediate e-stop */
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
EspNowPacket pkt = {}; EspNowPacket pkt = {};
pkt.magic[0] = ESPNOW_MAGIC_0; pkt.magic[0] = ESPNOW_MAGIC_0;
@ -366,11 +625,9 @@ static void estop_check(void) {
esp_now_send(broadcast_mac, (uint8_t *)&pkt, sizeof(pkt)); esp_now_send(broadcast_mac, (uint8_t *)&pkt, sizeof(pkt));
delay(5); delay(5);
} }
/* Also send fall event */
espnow_send_imu(&g_imu, MSG_FALL); espnow_send_imu(&g_imu, MSG_FALL);
} }
/* Auto-clear after cooldown */
if (g_estop_active && !g_fall_detected) { if (g_estop_active && !g_fall_detected) {
g_estop_active = false; g_estop_active = false;
Serial.println("+ESTOP:CLEAR"); Serial.println("+ESTOP:CLEAR");
@ -388,6 +645,7 @@ static void estop_check(void) {
/* ── Power Management ──────────────────────────────────────────── /* ── Power Management ────────────────────────────────────────────
* *
* Display timeout driven by g_cfg.sleep_timeout_s (BLE-configurable).
* Power states (cascading): * Power states (cascading):
* ACTIVE → display on, DW1000 ranging (~250mA) * ACTIVE → display on, DW1000 ranging (~250mA)
* DISPLAY_OFF → display off, DW1000 ranging (~225mA) * DISPLAY_OFF → display off, DW1000 ranging (~225mA)
@ -396,32 +654,25 @@ static void estop_check(void) {
* DEEP_SLEEP → ESP32 deep sleep (<0.5mA) * DEEP_SLEEP → ESP32 deep sleep (<0.5mA)
* Triggered by GPIO0 held 3s * Triggered by GPIO0 held 3s
* Wake by GPIO0 press (EXT0) * Wake by GPIO0 press (EXT0)
*
* Activity resets the idle timer and wakes display + DW1000.
* ─────────────────────────────────────────────────────────────── */ * ─────────────────────────────────────────────────────────────── */
#define PM_DISPLAY_TIMEOUT_MS (30UL * 1000) /* 30s → display off */
#define PM_DW1000_SLEEP_MS (300UL * 1000) /* 5min → DW1000 deep sleep */ #define PM_DW1000_SLEEP_MS (300UL * 1000) /* 5min → DW1000 deep sleep */
#define PM_DW1000_WAKE_PERIOD_MS (30UL * 1000) /* wake DW1000 every 30s */ #define PM_DW1000_WAKE_PERIOD_MS (30UL * 1000) /* wake DW1000 every 30s */
#define PM_DW1000_WAKE_WINDOW_MS 5000 /* stay awake 5s to range */ #define PM_DW1000_WAKE_WINDOW_MS 5000 /* stay awake 5s to range */
#define PM_DEEP_SLEEP_HOLD_MS 3000 /* GPIO0 hold → deep sleep */ #define PM_DEEP_SLEEP_HOLD_MS 3000 /* GPIO0 hold → deep sleep */
static uint32_t g_pm_last_activity_ms = 0; static uint32_t g_pm_last_activity_ms = 0;
static bool g_pm_display_on = true; static bool g_pm_display_on = true;
/* DW1000 deep sleep state */
static bool g_dw1000_sleeping = false; static bool g_dw1000_sleeping = false;
static uint32_t g_dw1000_sleep_ms = 0; /* when DW1000 entered sleep */ static uint32_t g_dw1000_sleep_ms = 0;
static uint32_t g_dw1000_wake_ms = 0; /* when current wake started */ static uint32_t g_dw1000_wake_ms = 0;
static bool g_dw1000_in_window = false; static bool g_dw1000_in_window = false;
/* GPIO0 hold tracking */
static uint32_t g_btn_held_ms = 0; static uint32_t g_btn_held_ms = 0;
/* Forward declaration */ static void dw1000_ranging_init(void); /* forward declaration */
static void dw1000_ranging_init(void);
/* Called on any ranging activity — resets idle timer, wakes hw */
static void pm_activity(void) { static void pm_activity(void) {
g_pm_last_activity_ms = millis(); g_pm_last_activity_ms = millis();
@ -440,7 +691,6 @@ static void pm_dw1000_sleep(void) {
g_dw1000_in_window = false; g_dw1000_in_window = false;
} }
/* Wake DW1000 and reinitialize ranging */
static void pm_dw1000_wake(void) { static void pm_dw1000_wake(void) {
Serial.println("[pm] DW1000 waking"); Serial.println("[pm] DW1000 waking");
DW1000.spiWakeup(); DW1000.spiWakeup();
@ -468,42 +718,39 @@ static void pm_enter_deep_sleep(void) {
delay(800); delay(800);
display.ssd1306_command(SSD1306_DISPLAYOFF); display.ssd1306_command(SSD1306_DISPLAYOFF);
/* DW1000 deep sleep before ESP32 sleeps */
if (!g_dw1000_sleeping) { if (!g_dw1000_sleeping) {
DW1000.deepSleep(); DW1000.deepSleep();
} }
/* Wake on GPIO0 low (button press) */
esp_sleep_enable_ext0_wakeup(GPIO_NUM_0, 0); esp_sleep_enable_ext0_wakeup(GPIO_NUM_0, 0);
esp_deep_sleep_start(); esp_deep_sleep_start();
/* never returns */ /* never returns */
} }
static void pm_update(void) { static void pm_update(void) {
uint32_t now = millis(); uint32_t now = millis();
uint32_t idle_ms = now - g_pm_last_activity_ms; uint32_t idle_ms = now - g_pm_last_activity_ms;
/* ── OLED auto-off ─────────────────────────────────────────── */ /* OLED auto-off — timeout from BLE-configurable sleep_timeout_s */
if (g_pm_display_on && idle_ms >= PM_DISPLAY_TIMEOUT_MS) { uint32_t display_timeout_ms = (uint32_t)g_cfg.sleep_timeout_s * 1000UL;
if (g_pm_display_on && idle_ms >= display_timeout_ms) {
display.ssd1306_command(SSD1306_DISPLAYOFF); display.ssd1306_command(SSD1306_DISPLAYOFF);
g_pm_display_on = false; g_pm_display_on = false;
Serial.println("[pm] Display off (30s idle)"); Serial.printf("[pm] Display off (%ds idle)\r\n", g_cfg.sleep_timeout_s);
} }
/* ── DW1000 sleep ──────────────────────────────────────────── */ /* DW1000 deep sleep */
if (!g_dw1000_sleeping) { if (!g_dw1000_sleeping) {
if (idle_ms >= PM_DW1000_SLEEP_MS) { if (idle_ms >= PM_DW1000_SLEEP_MS) {
pm_dw1000_sleep(); pm_dw1000_sleep();
} }
} else { } else {
/* Periodic scan window */ bool window_expired = g_dw1000_in_window &&
bool window_expired = g_dw1000_in_window && (now - g_dw1000_wake_ms) >= PM_DW1000_WAKE_WINDOW_MS;
(now - g_dw1000_wake_ms) >= PM_DW1000_WAKE_WINDOW_MS; bool period_elapsed = !g_dw1000_in_window &&
bool period_elapsed = !g_dw1000_in_window && (now - g_dw1000_sleep_ms) >= PM_DW1000_WAKE_PERIOD_MS;
(now - g_dw1000_sleep_ms) >= PM_DW1000_WAKE_PERIOD_MS;
if (window_expired) { if (window_expired) {
/* End of scan window with no activity → back to sleep */
Serial.println("[pm] DW1000 scan window done — back to sleep"); Serial.println("[pm] DW1000 scan window done — back to sleep");
pm_dw1000_sleep(); pm_dw1000_sleep();
} else if (period_elapsed) { } else if (period_elapsed) {
@ -511,13 +758,12 @@ static void pm_update(void) {
} }
} }
/* ── GPIO0 hold → deep sleep ───────────────────────────────── */ /* GPIO0 hold 3s → ESP32 deep sleep */
if (digitalRead(PIN_ESTOP) == LOW) { if (digitalRead(PIN_ESTOP) == LOW) {
if (g_btn_held_ms == 0) { if (g_btn_held_ms == 0) {
g_btn_held_ms = now; g_btn_held_ms = now;
} else if ((now - g_btn_held_ms) >= PM_DEEP_SLEEP_HOLD_MS) { } else if ((now - g_btn_held_ms) >= PM_DEEP_SLEEP_HOLD_MS) {
pm_enter_deep_sleep(); pm_enter_deep_sleep();
/* unreachable */
} }
} else { } else {
g_btn_held_ms = 0; g_btn_held_ms = 0;
@ -552,7 +798,7 @@ static void display_update(void) {
return; return;
} }
/* Show sleep status when DW1000 is sleeping */ /* Show sleep countdown when DW1000 is sleeping */
if (g_dw1000_sleeping && !g_dw1000_in_window) { if (g_dw1000_sleeping && !g_dw1000_in_window) {
uint32_t next_wake_ms = PM_DW1000_WAKE_PERIOD_MS - uint32_t next_wake_ms = PM_DW1000_WAKE_PERIOD_MS -
(millis() - g_dw1000_sleep_ms); (millis() - g_dw1000_sleep_ms);
@ -570,16 +816,22 @@ static void display_update(void) {
return; return;
} }
/* BLE connected indicator */
if (g_ble_connected) {
display.setTextSize(1);
display.setTextColor(SSD1306_WHITE);
display.setCursor(88, 0);
display.print(F("[BLE]"));
}
uint32_t now = millis(); uint32_t now = millis();
/* Find closest anchor */
int32_t min_range = INT32_MAX; int32_t min_range = INT32_MAX;
for (int i = 0; i < NUM_ANCHORS; i++) { for (int i = 0; i < NUM_ANCHORS; i++) {
if (g_anchor_range_mm[i] > 0 && g_anchor_range_mm[i] < min_range) if (g_anchor_range_mm[i] > 0 && g_anchor_range_mm[i] < min_range)
min_range = g_anchor_range_mm[i]; min_range = g_anchor_range_mm[i];
} }
/* Line 1: Big distance */
display.setTextSize(3); display.setTextSize(3);
display.setTextColor(SSD1306_WHITE); display.setTextColor(SSD1306_WHITE);
display.setCursor(0, 0); display.setCursor(0, 0);
@ -593,7 +845,6 @@ static void display_update(void) {
display.println(F("---")); display.println(F("---"));
} }
/* Line 2: Both anchor ranges */
display.setTextSize(1); display.setTextSize(1);
display.setCursor(0, 28); display.setCursor(0, 28);
for (int i = 0; i < NUM_ANCHORS; i++) { for (int i = 0; i < NUM_ANCHORS; i++) {
@ -604,23 +855,16 @@ static void display_update(void) {
} }
} }
/* Line 3: IMU data or link status */
display.setCursor(0, 40); display.setCursor(0, 40);
if (g_imu_ok) { if (g_imu_ok) {
/* Show accel magnitude + tilt indicator */
display.printf("%.1fg", g_imu.accel_mag); 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 tilt_x = constrain((int)(g_imu.ax * 20.0f), -30, 30);
int bar_center = 80; int bar_center = 80;
display.drawFastHLine(bar_center - 30, 44, 60, SSD1306_WHITE); display.drawFastHLine(bar_center - 30, 44, 60, SSD1306_WHITE);
display.fillRect(bar_center + tilt_x - 2, 42, 4, 4, SSD1306_WHITE); display.fillRect(bar_center + tilt_x - 2, 42, 4, 4, SSD1306_WHITE);
/* Gyro Z (yaw rate) */
display.setCursor(100, 40); display.setCursor(100, 40);
display.printf("%+.0f", g_imu.gz); display.printf("%+.0f", g_imu.gz);
} else { } else {
/* No IMU — show link status */
bool any_linked = false; bool any_linked = false;
for (int i = 0; i < NUM_ANCHORS; i++) { for (int i = 0; i < NUM_ANCHORS; i++) {
if (g_anchor_last_ok[i] > 0 && (now - g_anchor_last_ok[i]) < 2000) { if (g_anchor_last_ok[i] > 0 && (now - g_anchor_last_ok[i]) < 2000) {
@ -631,7 +875,6 @@ static void display_update(void) {
display.println(any_linked ? F("LINKED") : F("SEARCHING...")); display.println(any_linked ? F("LINKED") : F("SEARCHING..."));
} }
/* Line 4: Uptime + seq + IMU status */
display.setCursor(0, 54); display.setCursor(0, 54);
uint32_t secs = now / 1000; uint32_t secs = now / 1000;
display.printf("UP%02d:%02d p:%d", secs / 60, secs % 60, g_seq); display.printf("UP%02d:%02d p:%d", secs / 60, secs % 60, g_seq);
@ -670,12 +913,10 @@ static void newRange(void) {
delay(1); delay(1);
digitalWrite(PIN_LED, LOW); digitalWrite(PIN_LED, LOW);
/* Range received → mark activity, wake display/DW1000 if needed */ /* Activity: reset idle timer, wake display, exit DW1000 scan window */
pm_activity(); pm_activity();
/* If DW1000 was in a scan window, it's awake — keep it up */
if (g_dw1000_in_window) { if (g_dw1000_in_window) {
g_dw1000_in_window = false; /* anchor found, stay fully active */ g_dw1000_in_window = false;
} }
} }
@ -687,7 +928,7 @@ static void inactiveDevice(DW1000Device *device) {
Serial.printf("+GONE:%04X\r\n", device->getShortAddress()); Serial.printf("+GONE:%04X\r\n", device->getShortAddress());
} }
/* ── DW1000 init helper (used at startup and after wakeup) ─────── */ /* ── DW1000 init helper (startup + wakeup) ──────────────────────── */
static void dw1000_ranging_init(void) { static void dw1000_ranging_init(void) {
DW1000Ranging.initCommunication(PIN_RST, PIN_CS, PIN_IRQ); DW1000Ranging.initCommunication(PIN_RST, PIN_CS, PIN_IRQ);
@ -707,7 +948,6 @@ void setup(void) {
pinMode(PIN_LED, OUTPUT); pinMode(PIN_LED, OUTPUT);
digitalWrite(PIN_LED, LOW); digitalWrite(PIN_LED, LOW);
/* Log wakeup cause after deep sleep */
esp_sleep_wakeup_cause_t cause = esp_sleep_get_wakeup_cause(); esp_sleep_wakeup_cause_t cause = esp_sleep_get_wakeup_cause();
if (cause == ESP_SLEEP_WAKEUP_EXT0) { if (cause == ESP_SLEEP_WAKEUP_EXT0) {
Serial.println("[pm] Woke from deep sleep via GPIO0"); Serial.println("[pm] Woke from deep sleep via GPIO0");
@ -715,6 +955,12 @@ void setup(void) {
Serial.printf("\r\n[uwb_tag] tag_id=0x%02X starting\r\n", TAG_ID); 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) */ /* I2C bus (shared: OLED @0x3C + MPU6050 @0x68) */
Wire.begin(PIN_SDA, PIN_SCL); Wire.begin(PIN_SDA, PIN_SCL);
@ -731,25 +977,22 @@ void setup(void) {
display.setCursor(0, 20); display.setCursor(0, 20);
display.printf("Tag 0x%02X", TAG_ID); display.printf("Tag 0x%02X", TAG_ID);
display.setCursor(0, 35); display.setCursor(0, 35);
display.println(F("DW1000 200m + ESP-NOW")); display.println(F("DW1000 + BLE + PM"));
display.setCursor(0, 50); display.setCursor(0, 50);
display.println(F("Init IMU...")); display.printf("BLE: %s", g_ble_device_name);
display.display(); display.display();
} }
/* Load NVS config (requires g_ble_device_name for default tag_name) */
cfg_load();
cfg_apply();
/* MPU6050 */ /* MPU6050 */
g_imu_ok = mpu6050_init(); g_imu_ok = mpu6050_init();
if (g_imu_ok) { if (g_imu_ok)
Serial.println("[uwb_tag] MPU6050 init OK (±8g, ±500dps)"); Serial.println("[uwb_tag] MPU6050 init OK (±8g, ±500dps)");
display.setCursor(0, 50); else
display.println(F("IMU OK! Searching..."));
display.display();
} else {
Serial.println("[uwb_tag] MPU6050 not found — running without IMU"); Serial.println("[uwb_tag] MPU6050 not found — running without IMU");
display.setCursor(0, 50);
display.println(F("No IMU. Searching..."));
display.display();
}
/* ESP-NOW */ /* ESP-NOW */
WiFi.mode(WIFI_STA); WiFi.mode(WIFI_STA);
@ -767,11 +1010,14 @@ void setup(void) {
Serial.println("[uwb_tag] WARN: ESP-NOW failed"); Serial.println("[uwb_tag] WARN: ESP-NOW failed");
} }
/* BLE */
ble_init();
/* DW1000 */ /* DW1000 */
SPI.begin(PIN_SCK, PIN_MISO, PIN_MOSI); SPI.begin(PIN_SCK, PIN_MISO, PIN_MOSI);
dw1000_ranging_init(); dw1000_ranging_init();
/* Init state */ /* Init anchor state */
for (int i = 0; i < NUM_ANCHORS; i++) { for (int i = 0; i < NUM_ANCHORS; i++) {
g_anchor_range_mm[i] = -1; g_anchor_range_mm[i] = -1;
g_anchor_rssi[i] = -100.0f; g_anchor_rssi[i] = -100.0f;
@ -781,9 +1027,9 @@ void setup(void) {
g_pm_last_activity_ms = millis(); g_pm_last_activity_ms = millis();
Serial.println("[uwb_tag] DW1000 ready MODE_LONGDATA_RANGE_LOWPOWER"); 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 + PM active");
Serial.printf("[pm] Timeouts: display=%ds DW1000=%ds deep_sleep=hold_%ds\r\n", Serial.printf("[pm] Timeouts: display=%ds DW1000=%ds deep_sleep=hold_%ds\r\n",
PM_DISPLAY_TIMEOUT_MS / 1000, g_cfg.sleep_timeout_s,
PM_DW1000_SLEEP_MS / 1000, PM_DW1000_SLEEP_MS / 1000,
PM_DEEP_SLEEP_HOLD_MS / 1000); PM_DEEP_SLEEP_HOLD_MS / 1000);
} }
@ -794,29 +1040,27 @@ static uint32_t g_last_hb = 0;
static uint32_t g_last_imu = 0; static uint32_t g_last_imu = 0;
void loop(void) { 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)) { if (g_imu_ok && (millis() - g_last_imu >= 20)) {
mpu6050_read(&g_imu); mpu6050_read(&g_imu);
fall_detect(&g_imu); fall_detect(&g_imu);
g_last_imu = millis(); g_last_imu = millis();
} }
/* E-stop (now triggered by fall detection) */
estop_check(); estop_check();
/* Heartbeat every 1s — includes IMU data if available */ /* Heartbeat every 1s */
if (millis() - g_last_hb >= 1000) { if (millis() - g_last_hb >= 1000) {
espnow_send(MSG_HEARTBEAT, 0xFF, 0, 0.0f); espnow_send(MSG_HEARTBEAT, 0xFF, 0, 0.0f);
g_last_hb = millis(); 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; static uint32_t s_last_imu_tx = 0;
if (g_imu_ok && (millis() - s_last_imu_tx >= 100)) { if (g_imu_ok && (millis() - s_last_imu_tx >= 100)) {
espnow_send_imu(&g_imu, MSG_IMU); espnow_send_imu(&g_imu, MSG_IMU);
s_last_imu_tx = millis(); s_last_imu_tx = millis();
/* Serial output for debugging / ROS */
Serial.printf("+IMU:%.2f,%.2f,%.2f,%.1f,%.1f,%.1f,%.2f\r\n", Serial.printf("+IMU:%.2f,%.2f,%.2f,%.1f,%.1f,%.1f,%.2f\r\n",
g_imu.ax, g_imu.ay, g_imu.az, g_imu.ax, g_imu.ay, g_imu.az,
g_imu.gx, g_imu.gy, g_imu.gz, g_imu.gx, g_imu.gy, g_imu.gz,
@ -824,9 +1068,12 @@ void loop(void) {
s_last_imu_tx = millis(); s_last_imu_tx = millis();
} }
/* DW1000 ranging — skip while DW1000 is in deep sleep */ /* DW1000 ranging — skip while sleeping, throttle by ranging_interval_ms */
if (!g_dw1000_sleeping && !g_estop_active) { static uint32_t s_last_range_tick = 0;
if (!g_dw1000_sleeping && !g_estop_active &&
(millis() - s_last_range_tick >= g_cfg.ranging_interval_ms)) {
DW1000Ranging.loop(); DW1000Ranging.loop();
s_last_range_tick = millis();
} }
/* Power management (display timeout, DW1000 sleep, GPIO0 deep sleep) */ /* Power management (display timeout, DW1000 sleep, GPIO0 deep sleep) */