Test script to validate if IMU is calibrated properly

[kristof]

This script is a diagnostic tool to verify your BNO055 IMU is working correctly and properly calibrated. Here's what it does:

Overview

The script runs three tests to validate IMU health:

┌────────────────────────────────────────┐
│       IMU CALIBRATION TEST             │
├────────────────────────────────────────┤
│  1. Calibration File Check             │
│  2. Static Test (robot still)          │
│  3. Gyro Response Test (optional)      │
└────────────────────────────────────────┘

---

Test 1: Calibration File Check

def check_calibration_file():
    """Check if calibration file exists."""
    calib_path = "imu_calib_data.pkl"
    if os.path.exists(calib_path):
        import pickle
        data = pickle.load(open(calib_path, 'rb'))
        print(f"✅ Calibration file found: {calib_path}")
        print(f"   Accel offsets: {data.get('offsets_accelerometer', 'N/A')}")
        print(f"   Gyro offsets:  {data.get('offsets_gyroscope', 'N/A')}")
        print(f"   Mag offsets:   {data.get('offsets_magnetometer', 'N/A')}")

What it checks: Does imu_calib_data.pkl exist?

This file contains the sensor offsets that compensate for manufacturing variations. Without it, the IMU runs uncalibrated.

---

Test 2: Static Test

def test_static(imu, duration=3.0, samples=50):
    """Test IMU when robot is still and upright."""

Procedure: Keep the robot perfectly still and upright for 3 seconds.

What it measures:

Metric	What's Checked	Pass Criteria
Gyro mean	Drift/bias	< 0.1 rad/s on all axes
Gyro std	Noise level	< 0.05 rad/s
Accel magnitude	Gravity reading	9.81 ± 0.5 m/s²
Z-accel	Upright detection	> 8.0 m/s² (gravity on Z)
X/Y accel	Level bias	< 1.5 m/s² when level

What each check tells you:

• Gyro drift → If non-zero when still, the gyro thinks you're rotating when you're not
• Accel magnitude → Should equal gravity (9.81 m/s²) — wrong value means bad calibration
• Z-axis dominance → When upright, gravity should be mainly on Z-axis
• X/Y bias → Should be near zero when level

---

Test 3: Gyro Response Test (Interactive)

def test_gyro_response(imu):
    """Test gyro responds to rotation."""
    axes = ['X (roll)', 'Y (pitch)', 'Z (yaw)']
    
    for i, axis in enumerate(axes):
        input(f"\nPress Enter, then ROTATE around {axis} axis...")
        
        max_rate = 0
        for _ in range(30):
            data = imu.get_data()
            rate = abs(data['gyro'][i])
            max_rate = max(max_rate, rate)
            time.sleep(0.05)
        
        if max_rate > 0.5:
            print(f"✅ {axis}: Detected rotation ({max_rate:.2f} rad/s)")

Procedure: You manually rotate the robot around each axis when prompted.

What it checks:

• X (roll) → Tilt left/right
• Y (pitch) → Tilt forward/backward
• Z (yaw) → Spin/turn

Pass criteria: Gyro must detect > 0.5 rad/s rotation on the correct axis.

This catches issues like:

• Dead gyro axes
• Wrong axis mapping
• Sensor not responding

---

Output Example

==================================================
   SUMMARY
==================================================
✅ All tests passed! IMU appears properly calibrated.

Or if there are problems:

⚠️  Found 2 issue(s):
   ⚠️  Gyro drift detected: [ 0.15  0.02 -0.01]
   ⚠️  Accel magnitude off: 10.45 (expected ~9.81)

Consider running: python calibrate_imu.py

---

When to Run This

• After assembling a new duck
• If the robot seems unstable/wobbly
• After replacing the IMU
• If imu_calib_data.pkl was deleted
• To verify calibration before a demo