sl-firmware
87b45e1b97
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>
100 lines
3.7 KiB
C
100 lines
3.7 KiB
C
#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 */
|