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

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

Browse Source 
- 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
This commit is contained in:
Sebastien Vayrette 2026-03-14 14:09:39 -04:00
parent 6807f418ee
commit 6ea6e0ccd1
3 changed files with 304 additions and 81 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
esp32/.gitignore vendored Normal file
View File

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

23
esp32/uwb_anchor/src/main.cpp
View File

@ -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);

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

@ -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++) {
/* 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; /* 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.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,8 +434,23 @@ static void display_update(void) {
}
}
/* Line 3: Link status + RSSI bars */
display.setCursor(0, 42);
/* 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) {
@ -274,33 +458,17 @@ static void display_update(void) {
break;
}
}
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 --"));
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);
}