diff --git a/include/bno055.h b/include/bno055.h new file mode 100644 index 0000000..e0cae23 --- /dev/null +++ b/include/bno055.h @@ -0,0 +1,99 @@ +#ifndef BNO055_H +#define BNO055_H + +#include +#include +#include "mpu6000.h" /* IMUData */ + +/* + * BNO055 NDOF IMU driver over I2C1 (shared bus — PB8=SCL, PB9=SDA). + * + * Issue #135: auto-detected alongside MPU6000. Acts as: + * PRIMARY — when MPU6000 init fails (seamless fallback) + * AUGMENT — when both present; BNO055 provides better NDOF-fused yaw + * + * I2C addresses probed: 0x28 (ADR=0, default) then 0x29 (ADR=1). + * Chip-ID register 0x00 must read 0xA0. + * + * Operating mode: NDOF (0x0C) — 9DOF fusion with magnetometer. + * Falls back to IMUPLUS (0x08, no mag) if mag calibration stalls. + * + * Calibration offsets are saved to/restored from STM32 RTC backup + * registers (BKP0R–BKP6R = 28 bytes), identified by a magic word. + * If valid offsets are present, bno055_is_ready() returns true + * immediately after init. Otherwise, waits for gyro+accel cal ≥ 2. + * + * Temperature compensation is handled internally by the BNO055 silicon + * (it compensates all three sensors continuously). bno055_temperature() + * exposes the onboard thermometer reading for telemetry. + * + * Loop-rate note: BNO055 reads over I2C1 @100kHz take ~3ms, so the + * main balance loop drops from ~1kHz (MPU6000/SPI) to ~250Hz when + * BNO055 is active. 250Hz is sufficient for stable self-balancing. + * PID gain tuning may be required when switching IMU sources. + */ + +/* ---- Calibration status nibble masks (CALIB_STAT reg 0x35) ---- */ +#define BNO055_CAL_SYS_MASK 0xC0u /* bits [7:6] — overall system */ +#define BNO055_CAL_GYR_MASK 0x30u /* bits [5:4] — gyroscope */ +#define BNO055_CAL_ACC_MASK 0x0Cu /* bits [3:2] — accelerometer */ +#define BNO055_CAL_MAG_MASK 0x03u /* bits [1:0] — magnetometer */ +/* Each field: 0=uncalibrated, 3=fully calibrated */ + +/* + * bno055_init() — probe I2C1 for BNO055, reset, enter NDOF mode, + * restore saved calibration offsets if present. + * Requires i2c1_init() already called. + * Returns 0 on success, -1 if not found. + * Blocks ~750ms (POR + mode-switch settle). + * Call BEFORE safety_init() (IWDG not yet running). + */ +int bno055_init(void); + +/* + * bno055_read(data) — fill IMUData from BNO055 NDOF fusion output. + * Uses Euler angles for pitch/roll/yaw and gyro registers for pitch_rate. + * Triggers one I2C burst read (~3ms at 100kHz). + * Call from main loop balance gate (not every loop iteration). + */ +void bno055_read(IMUData *data); + +/* + * bno055_is_ready() — true when BNO055 is suitable for balance arming. + * True immediately if offsets were restored from backup RAM. + * Otherwise true once gyro calibration ≥ 2 and accel ≥ 2. + */ +bool bno055_is_ready(void); + +/* + * bno055_calib_status() — raw CALIB_STAT byte. + * Use BNO055_CAL_*_MASK to extract individual sensor calibration levels. + * Returned value is updated lazily on each bno055_read() call. + */ +uint8_t bno055_calib_status(void); + +/* + * bno055_temperature() — onboard temperature in °C (gyro source). + * Updated once per second (every ~250 calls to bno055_read()). + * Range: -40..+85°C. Use for telemetry reporting only. + */ +int8_t bno055_temperature(void); + +/* + * bno055_save_offsets() — write current calibration offsets to + * STM32 RTC backup registers BKP0R–BKP6R (22 bytes + magic). + * Call once after sys+acc+gyr calibration all reach level 3. + * Returns true if successful, false if BNO055 not present. + * Temporarily switches to CONFIGMODE — do NOT call while armed. + */ +bool bno055_save_offsets(void); + +/* + * bno055_restore_offsets() — read offsets from RTC backup registers + * and write them to BNO055 hardware (in CONFIGMODE). + * Called automatically by bno055_init(). + * Returns true if valid offsets found and applied. + */ +bool bno055_restore_offsets(void); + +#endif /* BNO055_H */ diff --git a/src/bno055.c b/src/bno055.c new file mode 100644 index 0000000..a6a3d52 --- /dev/null +++ b/src/bno055.c @@ -0,0 +1,316 @@ +/* + * bno055.c — Bosch BNO055 NDOF IMU driver over I2C1 + * + * Issue #135: auto-detect alongside MPU6000; fallback + yaw augmentation. + * + * Calibration offsets are persisted in STM32F7 RTC backup registers + * (BKP0R–BKP6R) and restored automatically on next power-on, so the + * sensor re-enters a calibrated state immediately after reset. + * + * Internal temperature compensation: + * The BNO055 silicon compensates all three sensors (accel, gyro, mag) + * continuously against its onboard thermometer. No external correction + * is required. bno055_temperature() exposes the reading for telemetry. + */ + +#include "bno055.h" +#include "i2c1.h" +#include "config.h" +#include "stm32f7xx_hal.h" +#include +#include + +/* ---- I2C addresses (7-bit, shifted left 1 for HAL) ---- */ +#define BNO055_ADDR_LOW (0x28u << 1) +#define BNO055_ADDR_HIGH (0x29u << 1) +#define BNO055_CHIP_ID_VAL 0xA0u + +/* ---- Register map (page 0) ---- */ +#define REG_CHIP_ID 0x00u +#define REG_PAGE_ID 0x07u +#define REG_GYRO_X_LSB 0x14u /* 6 bytes: Gx, Gy, Gz */ +#define REG_EULER_H_LSB 0x1Au /* 6 bytes: H, R, P (each int16) */ +#define REG_LIA_X_LSB 0x28u /* 6 bytes: linear accel X,Y,Z */ +#define REG_GRV_X_LSB 0x2Eu /* 6 bytes: gravity X,Y,Z */ +#define REG_TEMP 0x34u +#define REG_CALIB_STAT 0x35u +#define REG_OPR_MODE 0x3Du +#define REG_PWR_MODE 0x3Eu +#define REG_SYS_TRIGGER 0x3Fu +#define REG_TEMP_SOURCE 0x40u +#define REG_OFFSET_BASE 0x55u /* 22 bytes: accel(6)+mag(6)+gyro(6)+radii(4) */ + +/* ---- Operating modes ---- */ +#define MODE_CONFIG 0x00u +#define MODE_IMUPLUS 0x08u /* 6DOF, no mag */ +#define MODE_NDOF 0x0Cu /* 9DOF with mag */ + +/* ---- RTC backup register magic for saved offsets ---- */ +/* + * Layout: BKP0R..BKP5R hold 22 bytes of calibration offsets (packed + * 4 bytes per register; BKP5R only uses its low 2 bytes). + * BKP6R holds the magic word to validate the stored data. + */ +#define BNO055_BKP_MAGIC 0xB055CA10u +#define BKP_REG(n) (*(volatile uint32_t *)(&RTC->BKP0R + (n))) + +/* ---- Scaling constants ---- */ +/* + * BNO055 default unit selection (UNIT_SEL=0x00): + * Euler angles: 1 degree = 16 LSB → divide by 16.0f + * Gyro rates: 1 °/s = 16 LSB → divide by 16.0f + * Accel/LIA/GRV: 1 m/s² = 100 LSB → divide by 100.0f + */ +#define EULER_SCALE 16.0f +#define GYRO_SCALE 16.0f +#define ACCEL_SCALE 100.0f +#define G_MS2 9.80665f /* m/s² per g */ + +/* ---- Module state ---- */ +static uint16_t s_addr = 0; +static bool s_present = false; +static bool s_offsets_ok = false; /* true if offsets restored */ +static uint8_t s_calib = 0; /* cached CALIB_STAT */ +static int8_t s_temp = 25; /* cached temperature °C */ +static uint16_t s_temp_ctr = 0; /* temp read divider */ + +/* ---- Low-level I2C helpers ---- */ +static uint8_t rd(uint8_t reg) +{ + uint8_t v = 0; + HAL_I2C_Mem_Read(&hi2c1, s_addr, reg, 1, &v, 1, 100); + return v; +} + +static void wr(uint8_t reg, uint8_t val) +{ + HAL_I2C_Mem_Write(&hi2c1, s_addr, reg, 1, &val, 1, 100); +} + +static void rdblk(uint8_t reg, uint8_t *buf, uint8_t len) +{ + HAL_I2C_Mem_Read(&hi2c1, s_addr, reg, 1, buf, len, 200); +} + +static void wrblk(uint8_t reg, const uint8_t *buf, uint8_t len) +{ + HAL_I2C_Mem_Write(&hi2c1, s_addr, reg, 1, (uint8_t *)buf, len, 200); +} + +/* ---- bno055_restore_offsets() ---- */ +bool bno055_restore_offsets(void) +{ + __HAL_RCC_PWR_CLK_ENABLE(); + HAL_PWR_EnableBkUpAccess(); + + if (BKP_REG(6) != BNO055_BKP_MAGIC) return false; + + /* Unpack 22 bytes from BKP0R..BKP5R */ + uint8_t offsets[22]; + for (int i = 0; i < 5; i++) { + uint32_t w = BKP_REG(i); + offsets[i * 4 + 0] = (uint8_t)(w & 0xFFu); + offsets[i * 4 + 1] = (uint8_t)((w >> 8) & 0xFFu); + offsets[i * 4 + 2] = (uint8_t)((w >> 16) & 0xFFu); + offsets[i * 4 + 3] = (uint8_t)((w >> 24) & 0xFFu); + } + uint32_t w5 = BKP_REG(5); + offsets[20] = (uint8_t)(w5 & 0xFFu); + offsets[21] = (uint8_t)((w5 >> 8) & 0xFFu); + + /* Write offsets — must be in CONFIG mode */ + wr(REG_OPR_MODE, MODE_CONFIG); + HAL_Delay(25); + wr(REG_PAGE_ID, 0x00); + wrblk(REG_OFFSET_BASE, offsets, 22); + /* Caller switches back to NDOF after this returns */ + return true; +} + +/* ---- bno055_save_offsets() ---- */ +bool bno055_save_offsets(void) +{ + if (!s_present) return false; + + /* Switch to CONFIG mode to read offsets */ + wr(REG_OPR_MODE, MODE_CONFIG); + HAL_Delay(25); + wr(REG_PAGE_ID, 0x00); + + uint8_t offsets[22]; + rdblk(REG_OFFSET_BASE, offsets, 22); + + /* Restore to NDOF */ + wr(REG_OPR_MODE, MODE_NDOF); + HAL_Delay(10); + + /* Pack into RTC backup registers */ + __HAL_RCC_PWR_CLK_ENABLE(); + HAL_PWR_EnableBkUpAccess(); + + for (int i = 0; i < 5; i++) { + BKP_REG(i) = ((uint32_t)offsets[i * 4 + 0]) + | ((uint32_t)offsets[i * 4 + 1] << 8) + | ((uint32_t)offsets[i * 4 + 2] << 16) + | ((uint32_t)offsets[i * 4 + 3] << 24); + } + BKP_REG(5) = (uint32_t)offsets[20] | ((uint32_t)offsets[21] << 8); + BKP_REG(6) = BNO055_BKP_MAGIC; + + return true; +} + +/* ---- bno055_init() ---- */ +int bno055_init(void) +{ + /* Probe 0x28 (ADR=0) first, then 0x29 (ADR=1) */ + const uint16_t candidates[2] = { BNO055_ADDR_LOW, BNO055_ADDR_HIGH }; + s_present = false; + for (int i = 0; i < 2; i++) { + s_addr = candidates[i]; + if (rd(REG_CHIP_ID) == BNO055_CHIP_ID_VAL) { + s_present = true; + break; + } + } + if (!s_present) return -1; + + /* Software reset via SYS_TRIGGER */ + wr(REG_PAGE_ID, 0x00); + wr(REG_SYS_TRIGGER, 0x20u); + HAL_Delay(700); /* POR settle time from datasheet */ + + /* Confirm chip ID after reset */ + if (rd(REG_CHIP_ID) != BNO055_CHIP_ID_VAL) { + s_present = false; + return -1; + } + + wr(REG_PAGE_ID, 0x00); + + /* CONFIGMODE before any register writes */ + wr(REG_OPR_MODE, MODE_CONFIG); + HAL_Delay(25); + + /* Normal power mode */ + wr(REG_PWR_MODE, 0x00u); + HAL_Delay(10); + + /* + * Unit selection (UNIT_SEL = 0x00, default): + * Euler = degrees, Gyro = dps, Accel = m/s², Temp = °C + * No write needed — defaults match what we want. + */ + + /* Temperature source = gyro (more stable than accel) */ + wr(REG_TEMP_SOURCE, 0x01u); + + /* Try to restore saved calibration offsets */ + s_offsets_ok = bno055_restore_offsets(); + + /* Enter NDOF fusion mode */ + wr(REG_OPR_MODE, MODE_NDOF); + HAL_Delay(20); + + s_calib = 0; + s_temp = 25; + s_temp_ctr = 0; + return 0; +} + +/* ---- bno055_read() ---- */ +/* + * Performs two I2C burst reads (~3ms total at 100kHz): + * 1. REG_GYRO_X_LSB → 12 bytes: gyro(6) + euler(6) + * 2. REG_LIA_X_LSB → 12 bytes: linear_accel(6) + gravity(6) + * + * BNO055 coordinate frame (NDOF, no remapping): + * X: right, Y: forward, Z: up + * Euler H: yaw (0–360°, clockwise from north) + * Euler R: roll (–180°..+180°, right bank positive) + * Euler P: pitch (–90°..+90°, nose up positive) + * Gyro Y: pitch rate (deg/s, nose-up positive) + * + * Linear accel X (m/s²): forward-backward acceleration (gravity removed) + * Gravity Z (m/s²): gravity component on Z axis (≈9.81 when level) + */ +void bno055_read(IMUData *data) +{ + uint8_t buf[12]; + + /* ---- Burst 1: Gyro (0x14–0x19) + Euler (0x1A–0x1F) ---- */ + rdblk(REG_GYRO_X_LSB, buf, 12); + + /* Gyro: bytes 0..5 = [Gx_L, Gx_H, Gy_L, Gy_H, Gz_L, Gz_H] + * Gy = pitch rate (Y axis = pitch axis in BNO055 frame) */ + float pitch_rate = (float)(int16_t)((uint16_t)buf[3] << 8 | buf[2]) / GYRO_SCALE; + + /* Euler: bytes 6..11 = [H_L, H_H, R_L, R_H, P_L, P_H] */ + float heading = (float)(int16_t)((uint16_t)buf[7] << 8 | buf[6]) / EULER_SCALE; + float roll = (float)(int16_t)((uint16_t)buf[9] << 8 | buf[8]) / EULER_SCALE; + float pitch = (float)(int16_t)((uint16_t)buf[11] << 8 | buf[10]) / EULER_SCALE; + + /* ---- Burst 2: Linear accel (0x28–0x2D) + Gravity (0x2E–0x33) ---- */ + rdblk(REG_LIA_X_LSB, buf, 12); + + /* Linear accel X (forward, m/s²) — gravity-compensated */ + float lia_x_ms2 = (float)(int16_t)((uint16_t)buf[1] << 8 | buf[0]) / ACCEL_SCALE; + + /* Gravity Z (up axis, m/s²) — should be ≈+9.81 when level */ + float grv_z_ms2 = (float)(int16_t)((uint16_t)buf[11] << 8 | buf[10]) / ACCEL_SCALE; + + /* Fill IMUData in same units as mpu6000_read() */ + data->pitch = pitch; + data->pitch_rate = pitch_rate; + data->roll = roll; + data->yaw = heading; + data->accel_x = lia_x_ms2 / G_MS2; /* m/s² → g */ + data->accel_z = grv_z_ms2 / G_MS2; /* m/s² → g (≈1.0 when level) */ + + /* Periodically update calibration status and temperature (~1Hz at 250Hz loop) */ + if (++s_temp_ctr >= 250u) { + s_temp_ctr = 0; + s_calib = rd(REG_CALIB_STAT); + s_temp = (int8_t)rd(REG_TEMP); + + /* + * Auto-save offsets once when full calibration is achieved for + * the first time this session (sys=3, acc=3, gyr=3, don't wait + * for mag=3 as it is harder to achieve and not critical for balance). + */ + static bool s_saved = false; + if (!s_saved) { + uint8_t sys_cal = (s_calib & BNO055_CAL_SYS_MASK) >> 6; + uint8_t acc_cal = (s_calib & BNO055_CAL_ACC_MASK) >> 2; + uint8_t gyr_cal = (s_calib & BNO055_CAL_GYR_MASK) >> 4; + if (sys_cal >= 1u && acc_cal >= 3u && gyr_cal >= 3u) { + bno055_save_offsets(); + s_saved = true; + } + } + } +} + +/* ---- bno055_is_ready() ---- */ +bool bno055_is_ready(void) +{ + if (!s_present) return false; + /* If we restored saved offsets, trust them immediately */ + if (s_offsets_ok) return true; + /* Otherwise require gyro ≥ 2 and accel ≥ 2 */ + uint8_t acc_cal = (s_calib & BNO055_CAL_ACC_MASK) >> 2; + uint8_t gyr_cal = (s_calib & BNO055_CAL_GYR_MASK) >> 4; + return (acc_cal >= 2u) && (gyr_cal >= 2u); +} + +/* ---- bno055_calib_status() ---- */ +uint8_t bno055_calib_status(void) +{ + return s_calib; +} + +/* ---- bno055_temperature() ---- */ +int8_t bno055_temperature(void) +{ + return s_temp; +} diff --git a/src/main.c b/src/main.c index be97481..d61e7fe 100644 --- a/src/main.c +++ b/src/main.c @@ -218,7 +218,7 @@ int main(void) { if (jlink_state.arm_req) { jlink_state.arm_req = 0u; if (!safety_remote_estop_active() && - mpu6000_is_calibrated() && + imu_calibrated() && bal.state == BALANCE_DISARMED && fabsf(bal.pitch_deg) < 10.0f) { safety_arm_start(now); } @@ -260,7 +260,7 @@ int main(void) { if (rc_armed_now && !s_rc_armed_prev) { /* Rising edge: start arm hold (motors enable after ARMING_HOLD_MS) */ if (!safety_remote_estop_active() && - mpu6000_is_calibrated() && + imu_calibrated() && bal.state == BALANCE_DISARMED && fabsf(bal.pitch_deg) < 10.0f) { safety_arm_start(now); } @@ -277,7 +277,7 @@ int main(void) { if (cdc_arm_request) { cdc_arm_request = 0; if (!safety_remote_estop_active() && - mpu6000_is_calibrated() && + imu_calibrated() && bal.state == BALANCE_DISARMED && fabsf(bal.pitch_deg) < 10.0f) { safety_arm_start(now); } @@ -315,9 +315,14 @@ int main(void) { jetson_cmd_process(); } - /* Balance PID at 1kHz — apply Jetson speed offset when active. - * jlink takes priority; fall back to legacy CDC jetson_cmd if alive. */ + /* + * Balance PID — 1kHz with MPU6000/SPI, ~250Hz with BNO055/I2C. + * BNO055 read (~3ms) is placed inside the gate so it doesn't block + * the full main loop; the gap between ticks is naturally longer. + * jlink takes priority for Jetson speed offset; falls back to CDC. + */ if (imu_ret == 0 && now - balance_tick >= 1) { + if (bno055_active) bno055_read(&imu); /* I2C read inside gate */ balance_tick = now; float base_sp = bal.setpoint; if (jlink_is_active(now)) @@ -397,7 +402,7 @@ int main(void) { } /* USB telemetry at 50Hz (only when streaming enabled and calibration done) */ - if (cdc_streaming && mpu6000_is_calibrated() && now - send_tick >= 20) { + if (cdc_streaming && imu_calibrated() && now - send_tick >= 20) { send_tick = now; if (imu_ret == 0) { float err = bal.setpoint - bal.pitch_deg; @@ -458,6 +463,13 @@ int main(void) { if (_rs >= ESTOP_REMOTE) es = (int)_rs; else if (bal.state == BALANCE_TILT_FAULT) es = ESTOP_TILT; else es = ESTOP_CLEAR; + /* BNO055 calibration status and temperature when active */ + if (bno055_active) { + uint8_t cs = bno055_calib_status(); + n = snprintf(p, rem, ",\"bno_cs\":%d,\"bno_t\":%d", + (int)cs, (int)bno055_temperature()); + p += n; rem -= n; + } n = snprintf(p, rem, ",\"ja\":%d,\"jl\":%d,\"txc\":%u,\"rxc\":%u,\"es\":%d}\n", jetson_cmd_is_active(now) ? 1 : 0, jlink_is_active(now) ? 1 : 0, diff --git a/test/test_bno055_data.py b/test/test_bno055_data.py new file mode 100644 index 0000000..0ca3cb0 --- /dev/null +++ b/test/test_bno055_data.py @@ -0,0 +1,447 @@ +""" +test_bno055_data.py — Issue #135: BNO055 driver unit tests. + +Tests data scaling, byte parsing, calibration status extraction, +calibration offset packing/unpacking, and temperature handling. + +No HAL or STM32 hardware required — pure Python logic. +""" + +import struct +import pytest + + +# --------------------------------------------------------------------------- +# BNO055 scaling constants (mirror bno055.c) +# --------------------------------------------------------------------------- + +EULER_SCALE = 16.0 # 1 degree = 16 LSB +GYRO_SCALE = 16.0 # 1 dps = 16 LSB +ACCEL_SCALE = 100.0 # 1 m/s² = 100 LSB +G_MS2 = 9.80665 + + +# --------------------------------------------------------------------------- +# Calibration status bit extraction +# --------------------------------------------------------------------------- + +BNO055_CAL_SYS_MASK = 0xC0 +BNO055_CAL_GYR_MASK = 0x30 +BNO055_CAL_ACC_MASK = 0x0C +BNO055_CAL_MAG_MASK = 0x03 + + +def parse_calib_status(byte): + """Return (sys, gyr, acc, mag) calibration levels 0-3.""" + sys = (byte & BNO055_CAL_SYS_MASK) >> 6 + gyr = (byte & BNO055_CAL_GYR_MASK) >> 4 + acc = (byte & BNO055_CAL_ACC_MASK) >> 2 + mag = (byte & BNO055_CAL_MAG_MASK) >> 0 + return sys, gyr, acc, mag + + +# --------------------------------------------------------------------------- +# Offset packing (mirrors bno055_save_offsets / bno055_restore_offsets) +# --------------------------------------------------------------------------- + +BNO055_BKP_MAGIC = 0xB055CA10 + + +def pack_offsets(offsets22: bytes): + """Pack 22 bytes into 6 x uint32 words + magic word (7 words total).""" + assert len(offsets22) == 22 + words = [] + for i in range(5): + w = (offsets22[i * 4 + 0] + | (offsets22[i * 4 + 1] << 8) + | (offsets22[i * 4 + 2] << 16) + | (offsets22[i * 4 + 3] << 24)) + words.append(w & 0xFFFFFFFF) + # Last word: only bytes 20 and 21 + w5 = offsets22[20] | (offsets22[21] << 8) + words.append(w5 & 0xFFFFFFFF) + words.append(BNO055_BKP_MAGIC) + return words # 7 uint32 words + + +def unpack_offsets(words): + """Reverse of pack_offsets. Returns 22 bytes.""" + assert len(words) == 7 + assert words[6] == BNO055_BKP_MAGIC + result = [] + for i in range(5): + w = words[i] + result.append(w & 0xFF) + result.append((w >> 8) & 0xFF) + result.append((w >> 16) & 0xFF) + result.append((w >> 24) & 0xFF) + # Last word: only bytes 20 and 21 + result.append(words[5] & 0xFF) + result.append((words[5] >> 8) & 0xFF) + return bytes(result) + + +# --------------------------------------------------------------------------- +# IMUData parsing helpers (mirror bno055_read() logic) +# --------------------------------------------------------------------------- + +def parse_euler(buf6: bytes): + """Parse 6-byte Euler burst (H, R, P each int16 LE) → (heading, roll, pitch) in degrees.""" + h_raw = struct.unpack_from("= 2) and (gyr >= 2) + actual_ready = (acc >= 2) and (gyr >= 2) + assert actual_ready == expected_ready + + +# --------------------------------------------------------------------------- +# Tests: Euler Angle Parsing +# --------------------------------------------------------------------------- + +class TestEulerParsing: + + def test_zero(self): + buf = struct.pack("> 16) == 0 + + def test_byte_order_word0(self): + """First 4 bytes of offsets pack into word[0] as little-endian.""" + orig = bytes([0x01, 0x02, 0x03, 0x04] + [0] * 18) + words = pack_offsets(orig) + assert words[0] == 0x04030201 + + def test_byte_order_word5_low(self): + """Bytes 20-21 pack into low 16 bits of word[5].""" + orig = bytes([0] * 20 + [0xAB, 0xCD]) + words = pack_offsets(orig) + assert words[5] == 0xCDAB + + +# --------------------------------------------------------------------------- +# Tests: Temperature +# --------------------------------------------------------------------------- + +class TestTemperature: + + def test_room_temp_range(self): + """Room temperature should be in –40..+85°C range.""" + for t in [-40, 0, 25, 85]: + assert -40 <= t <= 85 + + def test_temperature_is_signed_byte(self): + """BNO055 TEMP register is signed int8 → –128..+127 but useful range –40..+85.""" + import ctypes + for raw in [25, 0x19, 0xFF]: + val = ctypes.c_int8(raw).value + assert -128 <= val <= 127 diff --git a/ui/social-bot/src/App.jsx b/ui/social-bot/src/App.jsx index 2c4c78a..0852320 100644 --- a/ui/social-bot/src/App.jsx +++ b/ui/social-bot/src/App.jsx @@ -1,11 +1,14 @@ /** - * App.jsx — Saltybot Social + Telemetry Dashboard root component. + * App.jsx — Saltybot Social + Telemetry + Fleet Dashboard root component. * * Social tabs (issue #107): * Status | Faces | Conversation | Personality | Navigation * * Telemetry tabs (issue #126): * IMU | Battery | Motors | Map | Control | Health + * + * Fleet tabs (issue #139): + * Fleet (self-contained via useFleet) */ import { useState, useCallback } from 'react'; @@ -26,6 +29,9 @@ import { MapViewer } from './components/MapViewer.jsx'; import { ControlMode } from './components/ControlMode.jsx'; import { SystemHealth } from './components/SystemHealth.jsx'; +// Fleet panel (issue #139) +import { FleetPanel } from './components/FleetPanel.jsx'; + const TAB_GROUPS = [ { label: 'SOCIAL', @@ -50,6 +56,13 @@ const TAB_GROUPS = [ { id: 'health', label: 'Health', }, ], }, + { + label: 'FLEET', + color: 'text-green-600', + tabs: [ + { id: 'fleet', label: 'Fleet' }, + ], + }, ]; const DEFAULT_WS_URL = 'ws://localhost:9090'; @@ -122,7 +135,9 @@ export default function App() { ⚡ SALTYBOT DASHBOARD - + {activeTab !== 'fleet' && ( + + )} {/* ── Tab Navigation ── */} @@ -172,14 +187,22 @@ export default function App() { {activeTab === 'map' && } {activeTab === 'control' && } {activeTab === 'health' && } + + {activeTab === 'fleet' && } {/* ── Footer ── */}
- rosbridge: {wsUrl} - - {connected ? 'CONNECTED' : 'DISCONNECTED'} - + {activeTab !== 'fleet' ? ( + <> + rosbridge: {wsUrl} + + {connected ? 'CONNECTED' : 'DISCONNECTED'} + + + ) : ( + FLEET MODE — multi-robot + )}
); diff --git a/ui/social-bot/src/components/FleetPanel.jsx b/ui/social-bot/src/components/FleetPanel.jsx new file mode 100644 index 0000000..063257e --- /dev/null +++ b/ui/social-bot/src/components/FleetPanel.jsx @@ -0,0 +1,803 @@ +/** + * FleetPanel.jsx — Multi-robot fleet management dashboard. + * + * Sub-views (internal tab nav): + * Robots | Map | Missions | Video | Alerts | Detail + * + * Uses useFleet() for multi-robot WebSocket multiplexing. + * Robot detail re-uses single-robot panel components via subscribeRobot adapter. + * + * Video streaming: + * http://:/stream?topic=&type=ros_compressed + * (web_video_server / mjpeg_server) + */ + +import { useState, useRef, useCallback, useEffect } from 'react'; +import { useFleet } from '../hooks/useFleet.js'; + +// Reusable single-robot detail panels (re-used via subscribeRobot adapter) +import { ImuPanel } from './ImuPanel.jsx'; +import { BatteryPanel } from './BatteryPanel.jsx'; +import { MotorPanel } from './MotorPanel.jsx'; +import { ControlMode } from './ControlMode.jsx'; +import { SystemHealth } from './SystemHealth.jsx'; + +// ── Constants ─────────────────────────────────────────────────────────────── + +const VARIANT_COLORS = { + balance: 'text-cyan-400 border-cyan-800', + rover: 'text-green-400 border-green-800', + tank: 'text-amber-400 border-amber-800', + social: 'text-purple-400 border-purple-800', +}; + +const FLEET_VIEWS = [ + { id: 'robots', label: 'Robots' }, + { id: 'map', label: 'Fleet Map'}, + { id: 'missions', label: 'Missions' }, + { id: 'video', label: 'Video' }, + { id: 'alerts', label: 'Alerts' }, +]; + +// ── Add-Robot modal ────────────────────────────────────────────────────────── + +function AddRobotModal({ onAdd, onClose }) { + const [form, setForm] = useState({ + name: '', variant: 'balance', host: '', port: '9090', + videoPort: '8080', videoTopic: '/camera/panoramic/compressed', + }); + const set = (k, v) => setForm(f => ({ ...f, [k]: v })); + + const handleSubmit = (e) => { + e.preventDefault(); + if (!form.host.trim()) return; + onAdd({ + ...form, + port: Number(form.port), + videoPort: Number(form.videoPort), + }); + onClose(); + }; + + return ( +
+
+
+ ADD ROBOT + +
+
+
+ + + + + + +
+
+ + +
+
+
+
+ ); +} + +// ── Sub-view: Robots List ──────────────────────────────────────────────────── + +function SocBar({ soc }) { + const color = soc < 20 ? '#ef4444' : soc < 40 ? '#f59e0b' : '#22c55e'; + return ( +
+
+
+
+ {soc}% +
+ ); +} + +function RobotCard({ robot, connection, data, onSelect, onRemove }) { + const conn = connection ?? { connected: false, error: null }; + const d = data ?? {}; + const varColor = VARIANT_COLORS[robot.variant] ?? 'text-gray-400 border-gray-700'; + + return ( +
onSelect(robot.id)} + > + {/* Header row */} +
+
+ {robot.name || robot.id} + {robot.variant} + +
+ + {/* Address */} +
{robot.host}:{robot.port}
+ + {/* Telemetry row */} + {conn.connected ? ( +
+
+ Battery + +
+
+ State + {d.state ?? '—'} +
+
+ Pipeline + {d.pipeline ?? '—'} +
+ {d.pose && ( +
+ pos {d.pose.x.toFixed(1)},{d.pose.y.toFixed(1)} + {' '}hdg {(d.pose.yaw * 180 / Math.PI).toFixed(0)}° +
+ )} +
+ ) : ( +
+ {conn.error ? ⚠ {conn.error} : 'Connecting…'} +
+ )} + + {d.alerts?.length > 0 && ( +
+ {d.alerts.slice(0, 3).map((a, i) => ( + = 2 ? 'bg-red-950 border-red-800 text-red-400' : 'bg-amber-950 border-amber-800 text-amber-400' + }`}>{a.name} + ))} +
+ )} +
+ ); +} + +function RobotsView({ robots, connections, robotData, onSelect, onRemove, onAdd, onScan, scanning }) { + return ( +
+
+
+ FLEET — {robots.length} ROBOT{robots.length !== 1 ? 'S' : ''} +
+
+ + +
+
+ {robots.length === 0 ? ( +
+ No robots configured. Add one or scan the LAN. +
+ ) : ( +
+ {robots.map(r => ( + + ))} +
+ )} +
+ ); +} + +// ── Sub-view: Fleet Map ────────────────────────────────────────────────────── + +const ROBOT_COLORS = ['#06b6d4','#f97316','#22c55e','#a855f7','#f59e0b','#ec4899','#84cc16','#38bdf8']; + +function FleetMapView({ robots, robotData, onSelect }) { + const canvasRef = useRef(null); + const [zoom, setZoom] = useState(30); // px per metre + const [pan, setPan] = useState({ x: 0, y: 0 }); + const dragging = useRef(null); + + useEffect(() => { + const canvas = canvasRef.current; + if (!canvas) return; + const ctx = canvas.getContext('2d'); + const W = canvas.width, H = canvas.height; + + ctx.clearRect(0, 0, W, H); + ctx.fillStyle = '#050510'; + ctx.fillRect(0, 0, W, H); + + // Grid + ctx.strokeStyle = '#0d1b2a'; + ctx.lineWidth = 1; + const gridSpacing = zoom; + const offX = (W / 2 + pan.x) % gridSpacing; + const offY = (H / 2 + pan.y) % gridSpacing; + for (let x = offX; x < W; x += gridSpacing) { + ctx.beginPath(); ctx.moveTo(x, 0); ctx.lineTo(x, H); ctx.stroke(); + } + for (let y = offY; y < H; y += gridSpacing) { + ctx.beginPath(); ctx.moveTo(0, y); ctx.lineTo(W, y); ctx.stroke(); + } + + // Origin cross + const cx = W / 2 + pan.x; + const cy = H / 2 + pan.y; + ctx.strokeStyle = '#1e3a4a'; + ctx.lineWidth = 1; + ctx.beginPath(); ctx.moveTo(cx - 10, cy); ctx.lineTo(cx + 10, cy); ctx.stroke(); + ctx.beginPath(); ctx.moveTo(cx, cy - 10); ctx.lineTo(cx, cy + 10); ctx.stroke(); + + // Draw each robot that has pose data + robots.forEach((robot, i) => { + const d = robotData[robot.id]; + if (!d?.pose) return; + const color = ROBOT_COLORS[i % ROBOT_COLORS.length]; + + const rx = cx + d.pose.x * zoom; + const ry = cy - d.pose.y * zoom; + const arrowLen = Math.max(14, zoom * 0.5); + + // Heading line + ctx.strokeStyle = color; + ctx.lineWidth = 2; + ctx.beginPath(); + ctx.moveTo(rx, ry); + ctx.lineTo(rx + Math.cos(d.pose.yaw) * arrowLen, ry - Math.sin(d.pose.yaw) * arrowLen); + ctx.stroke(); + + // Body circle + ctx.fillStyle = color; + ctx.shadowBlur = 10; + ctx.shadowColor = color; + ctx.beginPath(); + ctx.arc(rx, ry, 7, 0, Math.PI * 2); + ctx.fill(); + ctx.shadowBlur = 0; + + // Label + ctx.fillStyle = color; + ctx.font = '9px monospace'; + ctx.textAlign = 'center'; + ctx.fillText(robot.name || robot.id, rx, ry - 12); + }); + }, [robots, robotData, zoom, pan]); + + const onMouseDown = e => { dragging.current = { x: e.clientX - pan.x, y: e.clientY - pan.y }; }; + const onMouseMove = e => { + if (!dragging.current) return; + setPan({ x: e.clientX - dragging.current.x, y: e.clientY - dragging.current.y }); + }; + const onMouseUp = () => { dragging.current = null; }; + + const botsWithPose = robots.filter(r => robotData[r.id]?.pose); + + return ( +
+
+
FLEET MAP
+
+ + {zoom.toFixed(0)}px/m + + +
+
+
+ +
+ {/* Legend */} +
+ {robots.map((r, i) => ( + + ))} +
+
+ ); +} + +// ── Sub-view: Mission Dispatch ─────────────────────────────────────────────── + +function MissionsView({ robots, connections, sendGoal, sendPatrol }) { + const [targetId, setTargetId] = useState(''); + const [mode, setMode] = useState('goal'); // 'goal' | 'patrol' + const [goalX, setGoalX] = useState(''); + const [goalY, setGoalY] = useState(''); + const [goalYaw, setGoalYaw] = useState('0'); + const [waypoints, setWaypoints] = useState(''); + const [sent, setSent] = useState(null); + + const connected = robots.filter(r => connections[r.id]?.connected); + + const handleSend = () => { + if (!targetId) return; + if (mode === 'goal') { + sendGoal(targetId, parseFloat(goalX), parseFloat(goalY), parseFloat(goalYaw) * Math.PI / 180); + setSent(`Goal → (${goalX}, ${goalY}) sent to ${targetId}`); + } else { + try { + const pts = waypoints.trim().split('\n').map(line => { + const [x, y, yaw = 0] = line.split(',').map(Number); + return { x, y, yaw: yaw * Math.PI / 180 }; + }); + sendPatrol(targetId, pts); + setSent(`Patrol (${pts.length} wpts) sent to ${targetId}`); + } catch { + setSent('Error parsing waypoints'); + } + } + setTimeout(() => setSent(null), 4000); + }; + + return ( +
+
MISSION DISPATCH
+ + {connected.length === 0 && ( +
+ No robots connected. Connect at least one robot to dispatch missions. +
+ )} + +
+ + +
+ {['goal', 'patrol'].map(m => ( + + ))} +
+ + {mode === 'goal' ? ( +
+ {[['X (m)', goalX, setGoalX], ['Y (m)', goalY, setGoalY], ['Yaw (°)', goalYaw, setGoalYaw]].map(([lbl, val, setter]) => ( + + ))} +
+ ) : ( +