From 6ea6e0ccd1dcd53f878f0c8388242d5db0518635 Mon Sep 17 00:00:00 2001 From: Sebastien Vayrette Date: Sat, 14 Mar 2026 14:09:39 -0400 Subject: [PATCH] feat(uwb): add MPU6050 IMU + fall detection to tag, IMU forwarding in anchor + ROS2 node MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 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 | 2 + esp32/uwb_anchor/src/main.cpp | 23 +++ esp32/uwb_tag/src/main.cpp | 360 ++++++++++++++++++++++++++-------- 3 files changed, 304 insertions(+), 81 deletions(-) create mode 100644 esp32/.gitignore diff --git a/esp32/.gitignore b/esp32/.gitignore new file mode 100644 index 0000000..968a41b --- /dev/null +++ b/esp32/.gitignore @@ -0,0 +1,2 @@ +.pio/ +.vscode/ diff --git a/esp32/uwb_anchor/src/main.cpp b/esp32/uwb_anchor/src/main.cpp index 3587bb0..e15cc3a 100644 --- a/esp32/uwb_anchor/src/main.cpp +++ b/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); diff --git a/esp32/uwb_tag/src/main.cpp b/esp32/uwb_tag/src/main.cpp index 50c280a..d83bbac 100644 --- a/esp32/uwb_tag/src/main.cpp +++ b/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 - * E-Stop: GPIO 0 (BOOT), active LOW + * I2C: SDA=4 SCL=5 OLED @0x3C, MPU6050 @0x68 + * 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 some Makerfabs boards. */ - return; - bool pressed = (digitalRead(PIN_ESTOP) == LOW); + /* GPIO 0 (BOOT) floats LOW on Makerfabs display board — disabled. + * Fall detection acts as automatic e-stop instead. */ - 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"); - } - - if (g_estop_active && pressed) { - if (millis() - g_estop_last_tx >= 100) { + Serial.println("+ESTOP:FALL"); + /* Send immediate e-stop */ + for (int i = 0; i < 3; i++) { 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) { - for (int i = 0; i < 3; i++) { - 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.seq_num = g_seq++; - pkt.timestamp_ms = millis(); - esp_now_send(broadcast_mac, (uint8_t *)&pkt, sizeof(pkt)); - delay(10); - } + /* Auto-clear after cooldown */ + 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; + pkt.seq_num = g_seq++; + pkt.timestamp_ms = millis(); + esp_now_send(broadcast_mac, (uint8_t *)&pkt, sizeof(pkt)); } } @@ -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.println(F("E-STOP")); + display.setCursor(10, 0); + display.println(F("!! FALL !!")); display.setTextSize(1); - display.setCursor(20, 48); - display.println(F("RELEASE TO CLEAR")); + display.setCursor(0, 24); + 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,42 +434,41 @@ static void display_update(void) { } } - /* Line 3: Link status + RSSI bars */ - display.setCursor(0, 42); - 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) { - any_linked = true; - break; - } - } + /* 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); - 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); + /* 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); - 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); + /* Gyro Z (yaw rate) */ + display.setCursor(100, 40); + display.printf("%+.0f", g_imu.gz); } else { - display.println(F("LOST -- searching --")); + /* 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) { + any_linked = true; + break; + } + } + display.println(any_linked ? F("LINKED") : F("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(); } - /* - * Time-division: match anchor slots (50ms each, 2 anchors). - * At each slot boundary, let DW1000Ranging rediscover. - * This ensures the tag ranges with whichever anchor is active. - */ + /* IMU broadcast at 10 Hz via ESP-NOW */ + 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, + 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); }