feat(firmware): BNO055 NDOF IMU driver on I2C1 (Issue #135)
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>
This commit is contained in:
parent
b09f17b787
commit
87b45e1b97
99
include/bno055.h
Normal file
99
include/bno055.h
Normal file
@ -0,0 +1,99 @@
|
||||
#ifndef BNO055_H
|
||||
#define BNO055_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#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 */
|
||||
316
src/bno055.c
Normal file
316
src/bno055.c
Normal file
@ -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 <string.h>
|
||||
#include <math.h>
|
||||
|
||||
/* ---- 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;
|
||||
}
|
||||
24
src/main.c
24
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,
|
||||
|
||||
447
test/test_bno055_data.py
Normal file
447
test/test_bno055_data.py
Normal file
@ -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("<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
|
||||
Loading…
x
Reference in New Issue
Block a user