sl-firmware
ad2b599420
Auto-detected alongside MPU6000. Acts as balance primary when MPU6000 fails, or provides NDOF-fused yaw/pitch when both sensors are present. - include/bno055.h: full API — bno055_init/read/is_ready/calib_status/ temperature/save_offsets/restore_offsets - src/bno055.c: I2C1 driver; probes 0x28/0x29, resets via SYS_TRIGGER, enters NDOF mode; 2-burst 12-byte reads (gyro+euler, LIA+gravity); Euler/gyro/accel scaling (÷16, ÷16, ÷100); auto-saves offsets to RTC backup regs BKP0R–BKP6R on first full cal; restores on boot (bno055_is_ready() returns true immediately); temperature updated 1Hz - include/config.h: BNO055_BKP_MAGIC = 0xB055CA10 - src/main.c: bno055_init() in I2C probe block (before IWDG); imu_calibrated() macro dispatches mpu6000_is_calibrated() vs bno055_is_ready(); BNO055 read deferred inside balance gate to avoid stalling main loop; USB JSON reports bno_cs (calib status) and bno_t (temperature) - test/test_bno055_data.py: 43 pytest tests (43/43 pass) — calib status bit extraction, Euler/gyro/accel scaling, burst parsing, offset round-trip packing, temperature signed-byte encoding Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
448 lines
15 KiB
Python
448 lines
15 KiB
Python
"""
|
||
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("<h", buf6, 0)[0]
|
||
r_raw = struct.unpack_from("<h", buf6, 2)[0]
|
||
p_raw = struct.unpack_from("<h", buf6, 4)[0]
|
||
return h_raw / EULER_SCALE, r_raw / EULER_SCALE, p_raw / EULER_SCALE
|
||
|
||
|
||
def parse_gyro(buf6: bytes):
|
||
"""Parse 6-byte gyro burst (Gx, Gy, Gz each int16 LE) → (gx, gy, gz) in dps."""
|
||
gx = struct.unpack_from("<h", buf6, 0)[0] / GYRO_SCALE
|
||
gy = struct.unpack_from("<h", buf6, 2)[0] / GYRO_SCALE
|
||
gz = struct.unpack_from("<h", buf6, 4)[0] / GYRO_SCALE
|
||
return gx, gy, gz
|
||
|
||
|
||
def parse_lia_x(buf2: bytes):
|
||
"""Parse 2-byte linear accel X (int16 LE) → m/s²."""
|
||
raw = struct.unpack_from("<h", buf2, 0)[0]
|
||
return raw / ACCEL_SCALE
|
||
|
||
|
||
def parse_grv_z(buf2: bytes):
|
||
"""Parse 2-byte gravity Z (int16 LE) → m/s²."""
|
||
raw = struct.unpack_from("<h", buf2, 0)[0]
|
||
return raw / ACCEL_SCALE
|
||
|
||
|
||
def ms2_to_g(ms2):
|
||
return ms2 / G_MS2
|
||
|
||
|
||
# Mirroring bno055_read() 12-byte burst parsing:
|
||
# buf[0:5] = gyro X, Y, Z
|
||
# buf[6:11] = euler H, R, P
|
||
def parse_burst1(buf12: bytes):
|
||
"""Parse 12-byte burst1 (gyro 6B + euler 6B) from REG_GYRO_X_LSB."""
|
||
gyro = buf12[0:6]
|
||
euler = buf12[6:12]
|
||
_, pitch_rate, _ = parse_gyro(gyro) # gy = pitch rate
|
||
heading, roll, pitch = parse_euler(euler)
|
||
return pitch, roll, heading, pitch_rate
|
||
|
||
|
||
def parse_burst2(buf12: bytes):
|
||
"""Parse 12-byte burst2 (LIA 6B + GRV 6B) from REG_LIA_X_LSB."""
|
||
lia_x_ms2 = struct.unpack_from("<h", buf12, 0)[0] / ACCEL_SCALE # LIA X
|
||
grv_z_ms2 = struct.unpack_from("<h", buf12, 10)[0] / ACCEL_SCALE # GRV Z (bytes 10-11)
|
||
return ms2_to_g(lia_x_ms2), ms2_to_g(grv_z_ms2)
|
||
|
||
|
||
# ---------------------------------------------------------------------------
|
||
# Tests: Calibration Status
|
||
# ---------------------------------------------------------------------------
|
||
|
||
class TestCalibStatus:
|
||
|
||
def test_all_uncalibrated(self):
|
||
sys, gyr, acc, mag = parse_calib_status(0x00)
|
||
assert sys == 0 and gyr == 0 and acc == 0 and mag == 0
|
||
|
||
def test_all_fully_calibrated(self):
|
||
sys, gyr, acc, mag = parse_calib_status(0xFF)
|
||
assert sys == 3 and gyr == 3 and acc == 3 and mag == 3
|
||
|
||
def test_sys_only(self):
|
||
sys, gyr, acc, mag = parse_calib_status(0xC0)
|
||
assert sys == 3 and gyr == 0 and acc == 0 and mag == 0
|
||
|
||
def test_gyr_only(self):
|
||
sys, gyr, acc, mag = parse_calib_status(0x30)
|
||
assert sys == 0 and gyr == 3 and acc == 0 and mag == 0
|
||
|
||
def test_acc_only(self):
|
||
sys, gyr, acc, mag = parse_calib_status(0x0C)
|
||
assert sys == 0 and gyr == 0 and acc == 3 and mag == 0
|
||
|
||
def test_mag_only(self):
|
||
sys, gyr, acc, mag = parse_calib_status(0x03)
|
||
assert sys == 0 and gyr == 0 and acc == 0 and mag == 3
|
||
|
||
def test_partially_calibrated(self):
|
||
# sys=2, gyr=3, acc=1, mag=0 → 0b10_11_01_00 = 0xB4
|
||
sys, gyr, acc, mag = parse_calib_status(0xB4)
|
||
assert sys == 2 and gyr == 3 and acc == 1 and mag == 0
|
||
|
||
def test_bno055_is_ready_logic(self):
|
||
"""bno055_is_ready: requires acc≥2 AND gyr≥2 (when no offsets restored)."""
|
||
for calib_byte in range(256):
|
||
_, gyr, acc, _ = parse_calib_status(calib_byte)
|
||
expected_ready = (acc >= 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("<hhh", 0, 0, 0)
|
||
h, r, p = parse_euler(buf)
|
||
assert h == 0.0 and r == 0.0 and p == 0.0
|
||
|
||
def test_heading_north(self):
|
||
buf = struct.pack("<hhh", 0, 0, 0)
|
||
h, _, _ = parse_euler(buf)
|
||
assert h == 0.0
|
||
|
||
def test_heading_east_90deg(self):
|
||
raw = int(90 * EULER_SCALE)
|
||
buf = struct.pack("<hhh", raw, 0, 0)
|
||
h, _, _ = parse_euler(buf)
|
||
assert abs(h - 90.0) < 0.001
|
||
|
||
def test_heading_360deg(self):
|
||
raw = int(360 * EULER_SCALE)
|
||
buf = struct.pack("<hhh", raw, 0, 0)
|
||
h, _, _ = parse_euler(buf)
|
||
assert abs(h - 360.0) < 0.001
|
||
|
||
def test_pitch_nose_up_5deg(self):
|
||
raw = int(5.0 * EULER_SCALE)
|
||
buf = struct.pack("<hhh", 0, 0, raw)
|
||
_, _, p = parse_euler(buf)
|
||
assert abs(p - 5.0) < 0.001
|
||
|
||
def test_pitch_nose_down_negative(self):
|
||
raw = int(-12.5 * EULER_SCALE)
|
||
buf = struct.pack("<hhh", 0, 0, raw)
|
||
_, _, p = parse_euler(buf)
|
||
assert abs(p - (-12.5)) < 0.001
|
||
|
||
def test_roll_positive(self):
|
||
raw = int(30.0 * EULER_SCALE)
|
||
buf = struct.pack("<hhh", 0, raw, 0)
|
||
_, r, _ = parse_euler(buf)
|
||
assert abs(r - 30.0) < 0.001
|
||
|
||
def test_fractional_degree(self):
|
||
"""0.0625 degree resolution (1/16)."""
|
||
raw = 1 # 1 LSB = 1/16 degree
|
||
buf = struct.pack("<hhh", raw, 0, 0)
|
||
h, _, _ = parse_euler(buf)
|
||
assert abs(h - (1.0 / EULER_SCALE)) < 1e-6
|
||
|
||
|
||
# ---------------------------------------------------------------------------
|
||
# Tests: Gyro Rate Parsing
|
||
# ---------------------------------------------------------------------------
|
||
|
||
class TestGyroParsing:
|
||
|
||
def test_zero_rates(self):
|
||
buf = struct.pack("<hhh", 0, 0, 0)
|
||
gx, gy, gz = parse_gyro(buf)
|
||
assert gx == 0.0 and gy == 0.0 and gz == 0.0
|
||
|
||
def test_pitch_rate_positive(self):
|
||
raw = int(45.0 * GYRO_SCALE) # 45 dps nose-up
|
||
buf = struct.pack("<hhh", 0, raw, 0)
|
||
_, gy, _ = parse_gyro(buf)
|
||
assert abs(gy - 45.0) < 0.001
|
||
|
||
def test_pitch_rate_negative(self):
|
||
raw = int(-90.0 * GYRO_SCALE)
|
||
buf = struct.pack("<hhh", 0, raw, 0)
|
||
_, gy, _ = parse_gyro(buf)
|
||
assert abs(gy - (-90.0)) < 0.001
|
||
|
||
def test_max_range_900dps(self):
|
||
raw = int(900 * GYRO_SCALE)
|
||
buf = struct.pack("<hhh", raw, 0, 0)
|
||
gx, _, _ = parse_gyro(buf)
|
||
assert abs(gx - 900.0) < 0.001
|
||
|
||
|
||
# ---------------------------------------------------------------------------
|
||
# Tests: Linear Accel + Gravity Parsing
|
||
# ---------------------------------------------------------------------------
|
||
|
||
class TestAccelParsing:
|
||
|
||
def test_zero_linear_accel(self):
|
||
buf = struct.pack("<h", 0)
|
||
assert parse_lia_x(buf) == 0.0
|
||
|
||
def test_forward_accel(self):
|
||
raw = int(1.0 * ACCEL_SCALE) # 1 m/s² forward
|
||
buf = struct.pack("<h", raw)
|
||
assert abs(parse_lia_x(buf) - 1.0) < 0.001
|
||
|
||
def test_level_gravity_z(self):
|
||
"""Level robot: gravity Z ≈ 9.80665 m/s²."""
|
||
raw = int(G_MS2 * ACCEL_SCALE)
|
||
buf = struct.pack("<h", raw)
|
||
assert abs(parse_grv_z(buf) - G_MS2) < 0.02
|
||
|
||
def test_gravity_to_g_conversion(self):
|
||
raw = int(G_MS2 * ACCEL_SCALE)
|
||
buf = struct.pack("<h", raw)
|
||
g_val = ms2_to_g(parse_grv_z(buf))
|
||
assert abs(g_val - 1.0) < 0.002
|
||
|
||
def test_lia_negative(self):
|
||
raw = int(-2.5 * ACCEL_SCALE)
|
||
buf = struct.pack("<h", raw)
|
||
assert abs(parse_lia_x(buf) - (-2.5)) < 0.001
|
||
|
||
|
||
# ---------------------------------------------------------------------------
|
||
# Tests: 12-Byte Burst Parsing (mirrors bno055_read internals)
|
||
# ---------------------------------------------------------------------------
|
||
|
||
class TestBurstParsing:
|
||
|
||
def _make_burst1(self, gx=0, gy=0, gz=0, heading=0.0, roll=0.0, pitch=0.0):
|
||
gyro = struct.pack("<hhh",
|
||
int(gx * GYRO_SCALE),
|
||
int(gy * GYRO_SCALE),
|
||
int(gz * GYRO_SCALE))
|
||
euler = struct.pack("<hhh",
|
||
int(heading * EULER_SCALE),
|
||
int(roll * EULER_SCALE),
|
||
int(pitch * EULER_SCALE))
|
||
return gyro + euler
|
||
|
||
def _make_burst2(self, lia_x_ms2=0.0, grv_z_ms2=G_MS2):
|
||
lia = struct.pack("<hhh", int(lia_x_ms2 * ACCEL_SCALE), 0, 0)
|
||
grv = struct.pack("<hhh", 0, 0, int(grv_z_ms2 * ACCEL_SCALE))
|
||
return lia + grv
|
||
|
||
def test_pitch_from_burst1(self):
|
||
buf = self._make_burst1(pitch=7.5)
|
||
pitch, _, _, _ = parse_burst1(buf)
|
||
assert abs(pitch - 7.5) < 0.001
|
||
|
||
def test_yaw_from_burst1_heading(self):
|
||
buf = self._make_burst1(heading=180.0)
|
||
_, _, yaw, _ = parse_burst1(buf)
|
||
assert abs(yaw - 180.0) < 0.001
|
||
|
||
def test_pitch_rate_from_burst1_gy(self):
|
||
buf = self._make_burst1(gy=30.0)
|
||
_, _, _, pitch_rate = parse_burst1(buf)
|
||
assert abs(pitch_rate - 30.0) < 0.001
|
||
|
||
def test_accel_x_g_from_burst2(self):
|
||
buf = self._make_burst2(lia_x_ms2=G_MS2) # 1g forward
|
||
accel_x_g, _ = parse_burst2(buf)
|
||
assert abs(accel_x_g - 1.0) < 0.002
|
||
|
||
def test_gravity_z_level_from_burst2(self):
|
||
buf = self._make_burst2(grv_z_ms2=G_MS2) # level
|
||
_, accel_z_g = parse_burst2(buf)
|
||
assert abs(accel_z_g - 1.0) < 0.002
|
||
|
||
def test_tilted_gravity(self):
|
||
"""Tilted 30°: gravity Z = cos(30°) × 9.80665 ≈ 8.495 m/s²."""
|
||
import math
|
||
grv_z = G_MS2 * math.cos(math.radians(30))
|
||
buf = self._make_burst2(grv_z_ms2=grv_z)
|
||
_, accel_z_g = parse_burst2(buf)
|
||
expected = grv_z / G_MS2
|
||
assert abs(accel_z_g - expected) < 0.01
|
||
|
||
|
||
# ---------------------------------------------------------------------------
|
||
# Tests: Calibration Offset Packing / Backup Register Round-trip
|
||
# ---------------------------------------------------------------------------
|
||
|
||
class TestOffsetPacking:
|
||
|
||
def _make_offsets(self, fill=0):
|
||
return bytes([fill] * 22)
|
||
|
||
def test_pack_length(self):
|
||
words = pack_offsets(self._make_offsets(0))
|
||
assert len(words) == 7
|
||
|
||
def test_magic_in_last_word(self):
|
||
words = pack_offsets(self._make_offsets(0))
|
||
assert words[6] == BNO055_BKP_MAGIC
|
||
|
||
def test_round_trip_zeros(self):
|
||
orig = self._make_offsets(0)
|
||
words = pack_offsets(orig)
|
||
recovered = unpack_offsets(words)
|
||
assert recovered == orig
|
||
|
||
def test_round_trip_all_ones(self):
|
||
orig = self._make_offsets(0xFF)
|
||
words = pack_offsets(orig)
|
||
recovered = unpack_offsets(words)
|
||
assert recovered == orig
|
||
|
||
def test_round_trip_sequential(self):
|
||
orig = bytes(range(22))
|
||
words = pack_offsets(orig)
|
||
recovered = unpack_offsets(words)
|
||
assert recovered == orig
|
||
|
||
def test_round_trip_real_offsets(self):
|
||
"""Simulate a realistic BNO055 calibration offset set."""
|
||
# accel offset X/Y/Z (int16 LE), mag offset X/Y/Z, gyro offset X/Y/Z, radii
|
||
data = struct.pack("<hhhhhhhhhhh",
|
||
-45, 12, 980, # accel offsets (mg units)
|
||
150, -200, 50, # mag offsets (LSB units)
|
||
3, -7, 2, # gyro offsets (dps×16)
|
||
1000, 800) # accel_radius, mag_radius
|
||
assert len(data) == 22
|
||
words = pack_offsets(data)
|
||
recovered = unpack_offsets(words)
|
||
assert recovered == data
|
||
|
||
def test_invalid_magic_rejected(self):
|
||
words = pack_offsets(bytes(range(22)))
|
||
words_bad = words[:6] + [0xDEADBEEF] # corrupt magic
|
||
with pytest.raises(AssertionError):
|
||
unpack_offsets(words_bad)
|
||
|
||
def test_only_22_bytes_preserved(self):
|
||
"""Words 5's high 2 bytes are unused — should be zero after unpack."""
|
||
orig = bytes(range(22))
|
||
words = pack_offsets(orig)
|
||
# High 2 bytes of word[5] should be zero (only 2 bytes used)
|
||
assert (words[5] >> 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
|