feat(uwb): add MPU6050 IMU + fall detection to tag, IMU forwarding in anchor + ROS2 node

- Tag: MPU6050 on shared I2C bus (SDA=4, SCL=5, addr 0x68)
- Accel ±8g, gyro ±500dps, read at 50Hz
- Fall detection: freefall+impact or sudden >3.5g
- Fall triggers auto e-stop (ESP-NOW broadcast)
- OLED shows tilt bar + accel magnitude
- New ESP-NOW packet types: MSG_IMU (0x40), MSG_FALL (0x50)
- IMU broadcast at 10Hz via ESP-NOW
- Anchor forwards +IMU: and +FALL: lines to Jetson serial
- ROS2 uwb_driver publishes sensor_msgs/Imu on /uwb/imu
- .gitignore for .pio build dirs

esp32/.gitignore

@@ -0,0 +1,2 @@
+.pio/
+.vscode/

esp32/uwb_anchor/src/main.cpp

@@ -59,6 +59,8 @@ static char ANCHOR_ADDR[] = "86:17:5B:D5:A9:9A:E2:01";
 #define MSG_RANGE         0x10
 #define MSG_ESTOP         0x20
 #define MSG_HEARTBEAT     0x30
+#define MSG_IMU           0x40
+#define MSG_FALL          0x50
 
 #pragma pack(push, 1)
 struct EspNowPacket {
@@ -172,6 +174,27 @@ static void espnow_process(void) {
             continue;
         }
 
+        /* Forward IMU and FALL packets to Jetson serial */
+        if (pkt.msg_type == MSG_IMU || pkt.msg_type == MSG_FALL) {
+            const uint8_t *raw = (const uint8_t *)&pkt;
+            Serial.printf("+%s:%d,",
+                          pkt.msg_type == MSG_FALL ? "FALL" : "IMU",
+                          pkt.tag_id);
+            /* Forward as hex for easy parsing: ax,ay,az,gx,gy,gz,mag */
+            int16_t ax = (int16_t)(raw[4] | (raw[5] << 8));
+            int16_t ay = (int16_t)(raw[6] | (raw[7] << 8));
+            int16_t az = (int16_t)(raw[8] | (raw[9] << 8));
+            int16_t gx = (int16_t)(raw[10] | (raw[11] << 8));
+            int16_t gy = (int16_t)(raw[12] | (raw[13] << 8));
+            int16_t gz = (int16_t)(raw[14] | (raw[15] << 8));
+            uint8_t mag = raw[16];
+            Serial.printf("%.3f,%.3f,%.3f,%.1f,%.1f,%.1f,%.1f\r\n",
+                          ax/1000.0f, ay/1000.0f, az/1000.0f,
+                          gx/10.0f, gy/10.0f, gz/10.0f,
+                          mag/10.0f);
+            continue;
+        }
+
         Serial.printf("+ESPNOW:%d,%02X,%ld,%.1f,%d,%02X,%d\r\n",
                       pkt.tag_id, pkt.msg_type, (long)pkt.range_mm,
                       pkt.rssi_dbm, pkt.battery_pct, pkt.flags, pkt.seq_num);

esp32/uwb_tag/src/main.cpp

@@ -3,6 +3,7 @@
  *
  * Hardware: Makerfabs ESP32 UWB Pro with Display
  *   DW1000 (200m range), SSD1306 128x64 OLED, LiPo battery
+ *   MPU6050 (accel+gyro) on shared I2C bus @0x68
  *
  * Worn by person on EUC while robot follows.
  * Ranges with 2 anchors on robot, shows distance on OLED,
@@ -11,19 +12,20 @@
  * OLED display (5 Hz):
  *   Line 1: Big distance to nearest anchor
  *   Line 2: Per-anchor ranges
- *   Line 3: Link status + RSSI bars
+ *   Line 3: Link status + RSSI bars (or IMU accel)
  *   Line 4: Uptime + packet count
  *
  * ESP-NOW broadcast (20-byte packets):
  *   Range data after each successful TWR cycle
  *   Heartbeat every 1s even if ranging fails
  *   E-stop at 10 Hz while button held
+ *   IMU data at 10 Hz (piggybacked on MSG_IMU)
  *
  * Pin map — ESP32 UWB Pro with Display:
  *   SPI: SCK=18 MISO=19 MOSI=23 CS=21 (not 4! display board differs)
  *   DW1000: RST=27 IRQ=34
- *   I2C: SDA=4 SCL=5  OLED @0x3C
+ *   I2C: SDA=4 SCL=5  OLED @0x3C, MPU6050 @0x68
- *   E-Stop: GPIO 0 (BOOT), active LOW
+ *   E-Stop: GPIO 0 (BOOT), active LOW — disabled (floats LOW)
  *   LED: GPIO 2
  */
 
@@ -68,6 +70,140 @@ static char TAG_ADDR[] = "7D:00:22:EA:82:60:3B:9B";
 
 Adafruit_SSD1306 display(128, 64, &Wire, -1);
 
+/* ── MPU6050 (I2C @0x68, shared bus with OLED) ──────────────────── */
+
+#define MPU6050_ADDR       0x68
+#define MPU6050_WHO_AM_I   0x75
+#define MPU6050_PWR_MGMT_1 0x6B
+#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 */
+
+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 */
+};
+
+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 */
+
+static bool     g_fall_detected = false;
+static uint32_t g_fall_last_ms = 0;
+static bool     g_freefall_phase = false;
+static uint32_t g_freefall_start = 0;
+
+static void mpu6050_write(uint8_t reg, uint8_t val) {
+    Wire.beginTransmission(MPU6050_ADDR);
+    Wire.write(reg);
+    Wire.write(val);
+    Wire.endTransmission();
+}
+
+static uint8_t mpu6050_read_byte(uint8_t reg) {
+    Wire.beginTransmission(MPU6050_ADDR);
+    Wire.write(reg);
+    Wire.endTransmission(false);
+    Wire.requestFrom((uint8_t)MPU6050_ADDR, (uint8_t)1);
+    return Wire.read();
+}
+
+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;
+}
+
+static void mpu6050_read(ImuData *d) {
+    Wire.beginTransmission(MPU6050_ADDR);
+    Wire.write(MPU6050_ACCEL_XOUT);
+    Wire.endTransmission(false);
+    Wire.requestFrom((uint8_t)MPU6050_ADDR, (uint8_t)14);
+
+    int16_t raw[7];
+    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) {
+                g_fall_detected = true;
+                g_fall_last_ms = now;
+                Serial.printf("+FALL:%.1fg freefall=%dms\r\n",
+                              d->accel_mag, now - g_freefall_start);
+            }
+        }
+        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;
+        Serial.printf("+FALL:%.1fg impact\r\n", d->accel_mag);
+    }
+}
+
 /* ── ESP-NOW ────────────────────────────────────────────────────── */
 
 #define ESPNOW_MAGIC_0  0x5B
@@ -75,6 +211,8 @@ Adafruit_SSD1306 display(128, 64, &Wire, -1);
 #define MSG_RANGE        0x10
 #define MSG_ESTOP        0x20
 #define MSG_HEARTBEAT    0x30
+#define MSG_IMU          0x40
+#define MSG_FALL         0x50
 
 static uint8_t broadcast_mac[6] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
 static uint8_t g_seq = 0;
@@ -93,6 +231,22 @@ struct EspNowPacket {
     uint8_t  seq_num;
     uint8_t  _pad;
 };
+
+/* Extended IMU packet — fits in 20 bytes too */
+struct EspNowImuPacket {
+    uint8_t  magic[2];       /* 0x5B 0x01 */
+    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  seq_num;
+    uint16_t timestamp_s10; /* uptime in 0.1s (wraps at ~109 min) */
+};
 #pragma pack(pop)
 
 /* ── Anchor tracking (declared early for ESP-NOW RX callback) ──── */
@@ -137,13 +291,26 @@ static void espnow_send(uint8_t msg_type, uint8_t anchor_id,
     esp_now_send(broadcast_mac, (uint8_t *)&pkt, sizeof(pkt));
 }
 
+static void espnow_send_imu(const ImuData *d, uint8_t msg_type) {
+    EspNowImuPacket pkt = {};
+    pkt.magic[0]      = ESPNOW_MAGIC_0;
+    pkt.magic[1]      = ESPNOW_MAGIC_1;
+    pkt.tag_id        = TAG_ID;
+    pkt.msg_type      = msg_type;
+    pkt.ax_mg         = (int16_t)(d->ax * 1000.0f);
+    pkt.ay_mg         = (int16_t)(d->ay * 1000.0f);
+    pkt.az_mg         = (int16_t)(d->az * 1000.0f);
+    pkt.gx_dps10      = (int16_t)(d->gx * 10.0f);
+    pkt.gy_dps10      = (int16_t)(d->gy * 10.0f);
+    pkt.gz_dps10      = (int16_t)(d->gz * 10.0f);
+    pkt.accel_mag_10  = (uint8_t)constrain((int)(d->accel_mag * 10.0f), 0, 255);
+    pkt.seq_num       = g_seq++;
+    pkt.timestamp_s10 = (uint16_t)((millis() / 100) & 0xFFFF);
+    esp_now_send(broadcast_mac, (uint8_t *)&pkt, sizeof(pkt));
+}
+
 /* ── Anchor tracking (DW1000 address mapping) ──────────────────── */
 
-/*
- * DW1000Ranging gives us callbacks with a device short address.
- * We map the first 2 unique addresses we see to anchor 0 and 1.
- * g_anchor_range_mm/rssi/last_ok declared above (before ESP-NOW RX callback).
- */
 static uint16_t g_anchor_addrs[NUM_ANCHORS] = {0, 0};
 static int      g_num_known_anchors = 0;
 
@@ -157,7 +324,7 @@ static int anchor_index(uint16_t addr) {
         Serial.printf("[uwb_tag] Mapped anchor %04X → A%d\r\n", addr, idx);
         return idx;
     }
-    return -1;  /* more anchors than expected */
+    return -1;
 }
 
 /* ── E-Stop ─────────────────────────────────────────────────────── */
@@ -166,18 +333,15 @@ static bool     g_estop_active = false;
 static uint32_t g_estop_last_tx = 0;
 
 static void estop_check(void) {
-    /* E-stop disabled until external button is wired.
+    /* GPIO 0 (BOOT) floats LOW on Makerfabs display board — disabled.
-     * GPIO 0 (BOOT) floats LOW on some Makerfabs boards. */
+     * Fall detection acts as automatic e-stop instead. */
-    return;
-    bool pressed = (digitalRead(PIN_ESTOP) == LOW);
 
-    if (pressed && !g_estop_active) {
+    /* Auto e-stop on fall detection */
+    if (g_fall_detected && !g_estop_active) {
         g_estop_active = true;
-        Serial.println("+ESTOP:ACTIVE");
+        Serial.println("+ESTOP:FALL");
-    }
+        /* Send immediate e-stop */
-
+        for (int i = 0; i < 3; i++) {
-    if (g_estop_active && pressed) {
-        if (millis() - g_estop_last_tx >= 100) {
             EspNowPacket pkt = {};
             pkt.magic[0] = ESPNOW_MAGIC_0;
             pkt.magic[1] = ESPNOW_MAGIC_1;
@@ -187,25 +351,25 @@ static void estop_check(void) {
             pkt.seq_num  = g_seq++;
             pkt.timestamp_ms = millis();
             esp_now_send(broadcast_mac, (uint8_t *)&pkt, sizeof(pkt));
-            g_estop_last_tx = millis();
+            delay(5);
         }
+        /* Also send fall event */
+        espnow_send_imu(&g_imu, MSG_FALL);
     }
 
-    if (!pressed && g_estop_active) {
+    /* Auto-clear after cooldown */
-        for (int i = 0; i < 3; i++) {
+    if (g_estop_active && !g_fall_detected) {
+        g_estop_active = false;
+        Serial.println("+ESTOP:CLEAR");
         EspNowPacket pkt = {};
         pkt.magic[0] = ESPNOW_MAGIC_0;
         pkt.magic[1] = ESPNOW_MAGIC_1;
         pkt.tag_id   = TAG_ID;
         pkt.msg_type = MSG_ESTOP;
-            pkt.flags    = 0x00;  /* clear */
+        pkt.flags    = 0x00;
         pkt.seq_num  = g_seq++;
         pkt.timestamp_ms = millis();
         esp_now_send(broadcast_mac, (uint8_t *)&pkt, sizeof(pkt));
-            delay(10);
-        }
-        g_estop_active = false;
-        Serial.println("+ESTOP:CLEAR");
     }
 }
 
@@ -220,13 +384,18 @@ static void display_update(void) {
     display.clearDisplay();
 
     if (g_estop_active) {
-        display.setTextSize(3);
+        display.setTextSize(2);
         display.setTextColor(SSD1306_WHITE);
-        display.setCursor(10, 4);
+        display.setCursor(10, 0);
-        display.println(F("E-STOP"));
+        display.println(F("!! FALL !!"));
         display.setTextSize(1);
-        display.setCursor(20, 48);
+        display.setCursor(0, 24);
-        display.println(F("RELEASE TO CLEAR"));
+        display.printf("Impact: %.1fg", g_imu.accel_mag);
+        display.setCursor(0, 36);
+        display.println(F("E-STOP ACTIVE"));
+        display.setCursor(0, 50);
+        display.printf("Clears in %ds",
+            (int)((FALL_COOLDOWN_MS - (millis() - g_fall_last_ms)) / 1000));
         display.display();
         return;
     }
@@ -256,7 +425,7 @@ static void display_update(void) {
 
     /* Line 2: Both anchor ranges */
     display.setTextSize(1);
-    display.setCursor(0, 30);
+    display.setCursor(0, 28);
     for (int i = 0; i < NUM_ANCHORS; i++) {
         if (g_anchor_range_mm[i] > 0) {
             display.printf("A%d:%.1fm ", i, g_anchor_range_mm[i] / 1000.0f);
@@ -265,8 +434,23 @@ static void display_update(void) {
         }
     }
 
-    /* Line 3: Link status + RSSI bars */
+    /* Line 3: IMU data or link status */
-    display.setCursor(0, 42);
+    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) {
@@ -274,33 +458,17 @@ static void display_update(void) {
                 break;
             }
         }
-
+        display.println(any_linked ? F("LINKED") : F("SEARCHING..."));
-    if (any_linked) {
-        float best_rssi = -100.0f;
-        for (int i = 0; i < NUM_ANCHORS; i++) {
-            if (g_anchor_rssi[i] > best_rssi) best_rssi = g_anchor_rssi[i];
-        }
-        int bars = constrain((int)((best_rssi + 90.0f) / 12.0f), 0, 5);
-
-        display.print(F("LINKED "));
-        for (int b = 0; b < 5; b++) {
-            int x = 50 + b * 6;
-            int h = 2 + b * 2;
-            int y = 50 - h;
-            if (b < bars)
-                display.fillRect(x, y, 4, h, SSD1306_WHITE);
-            else
-                display.drawRect(x, y, 4, h, SSD1306_WHITE);
-        }
-        display.printf(" %.0f", best_rssi);
-    } else {
-        display.println(F("LOST  -- searching --"));
     }
 
-    /* Line 4: Uptime + seq */
+    /* Line 4: Uptime + seq + IMU status */
     display.setCursor(0, 54);
     uint32_t secs = now / 1000;
-    display.printf("UP %02d:%02d  pkt:%d", secs / 60, secs % 60, g_seq);
+    display.printf("UP%02d:%02d p:%d", secs / 60, secs % 60, g_seq);
+    if (g_imu_ok) {
+        display.setCursor(100, 54);
+        display.print(F("IMU"));
+    }
 
     display.display();
 }
@@ -323,14 +491,11 @@ static void newRange(void) {
         g_anchor_last_ok[idx]  = millis();
     }
 
-    /* Serial debug */
     Serial.printf("+RANGE:%d,%04X,%ld,%.1f\r\n",
                   (idx >= 0 ? idx : 99), addr, (long)range_mm, rxPow);
 
-    /* ESP-NOW broadcast */
     espnow_send(MSG_RANGE, (uint8_t)(idx >= 0 ? idx : 0xFF), range_mm, rxPow);
 
-    /* LED blink */
     digitalWrite(PIN_LED, HIGH);
     delay(1);
     digitalWrite(PIN_LED, LOW);
@@ -356,8 +521,10 @@ void setup(void) {
 
     Serial.printf("\r\n[uwb_tag] tag_id=0x%02X  starting\r\n", TAG_ID);
 
-    /* OLED */
+    /* I2C bus (shared: OLED @0x3C + MPU6050 @0x68) */
     Wire.begin(PIN_SDA, PIN_SCL);
+
+    /* OLED */
     if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
         Serial.println("[uwb_tag] WARN: SSD1306 not found");
     } else {
@@ -372,7 +539,21 @@ void setup(void) {
         display.setCursor(0, 35);
         display.println(F("DW1000 200m + ESP-NOW"));
         display.setCursor(0, 50);
-        display.println(F("Searching anchors..."));
+        display.println(F("Init IMU..."));
+        display.display();
+    }
+
+    /* 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();
     }
 
@@ -416,22 +597,40 @@ void setup(void) {
 /* ── Loop ───────────────────────────────────────────────────────── */
 
 static uint32_t g_last_hb = 0;
+static uint32_t g_last_imu = 0;
 
 void loop(void) {
-    /* E-stop always first */
+    /* IMU read at ~50 Hz (fast enough for fall detection) */
+    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 */
+    /* Heartbeat every 1s — includes IMU data if available */
     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 */
-     * Time-division: match anchor slots (50ms each, 2 anchors).
+    static uint32_t s_last_imu_tx = 0;
-     * At each slot boundary, let DW1000Ranging rediscover.
+    if (g_imu_ok && (millis() - s_last_imu_tx >= 100)) {
-     * This ensures the tag ranges with whichever anchor is active.
+        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,
+                      g_imu.accel_mag);
+        s_last_imu_tx = millis();
+    }
+
+    /* DW1000 ranging */
     if (!g_estop_active) {
         DW1000Ranging.loop();
     }
@@ -439,6 +638,5 @@ void loop(void) {
     /* Display at 5 Hz */
     display_update();
 
-    /* Yield to prevent tight-loop when no anchors respond */
     delay(1);
 }