IMU Integration (QMI8658 + QMC6310) for LILYGO T-Beam Supreme with Madgwick Filter

[MATBckh22]

🤝 Attestations

• I have tested that my proposed changes behave as described.
• I have tested that my proposed changes do not cause any obvious regressions on the following devices:
  • Heltec (Lora32) V3
  • LilyGo T-Deck
  • LilyGo T-Beam
  • RAK WisBlock 4631
  • Seeed Studio T-1000E tracker card
  • Other (please specify below)

This implementation adds GPS-aided IMU sensor fusion using a Madgwick filter to combine GPS and IMU data for improved navigation accuracy.

Overview

The GPS+IMU fusion system combines:

• IMU data (accelerometer, gyroscope, magnetometer) for high-rate orientation and motion sensing
• GPS data (position, velocity, course) for absolute position reference and drift correction
• Madgwick AHRS filter (from the existing Fusion library) for orientation estimation

Key Features

• Complementary sensor fusion - GPS provides absolute position reference, IMU provides high-rate motion data
• GPS-aided heading - Uses GPS course when vehicle is moving to improve yaw accuracy
• Velocity fusion - Combines GPS velocity with IMU acceleration integration
• Quality assessment - Provides accuracy estimates based on GPS HDOP and motion state
• Automatic fallback - Uses pure IMU when GPS is unavailable

Data Structure

The fused data is available through GPSIMUFusionData:

struct GPSIMUFusionData {
    // Status
    bool initialized, gps_valid, imu_valid;
    
    // Position (GPS-based with IMU smoothing)
    double latitude, longitude;  // degrees
    float altitude;              // meters MSL
    
    // Velocity (GPS + IMU fusion)
    float velocity_north, velocity_east, velocity_down;  // m/s
    float speed;                 // horizontal speed m/s
    
    // Orientation (IMU-based with GPS heading aid)
    float roll, pitch, yaw;      // degrees
    
    // Quality indicators
    float hdop;                  // GPS horizontal dilution of precision
    uint8_t satellites;          // number of GPS satellites
    float heading_accuracy;      // estimated heading accuracy (degrees)
    
    // Timestamps
    uint32_t last_gps_ms, last_imu_ms, last_fusion_ms;
};

Usage

Accessing Fusion Data

#include "motion/SensorLiveData.h"

const GPSIMUFusionData* fusion = getGPSIMUFusionData();
if (fusion && fusion->initialized) {
    // Use fusion data
    float heading = fusion->yaw;
    float lat = fusion->latitude;
    float speed = fusion->speed;
}

Example Applications

1. Navigation Display

   • Use latitude/longitude for position
   • Use yaw for compass heading
   • Use speed for motion indication

2. Motion Detection

   • Check speed > threshold for movement detection
   • Use roll/pitch for tilt sensing

3. Quality-based Usage

   • High accuracy (HDOP < 2.0, sats >= 6): Precise navigation
   • Medium accuracy (HDOP < 5.0): General navigation
   • Low accuracy: Use with caution

Algorithm Details

GPS Integration

• Position: GPS provides absolute reference, filtered for smoothness
• Velocity: GPS velocity corrects IMU integration drift
• Heading: GPS course used when moving (speed > 1.0 m/s)

IMU Processing

• Orientation: Madgwick AHRS with gyro, accel, and magnetometer
• Integration: Acceleration integrated to velocity (with GPS correction)
• Quality: Magnetometer rejection based on external field strength

Fusion Strategy

• Complementary filtering for velocity (GPS corrects IMU drift)
• Heading fusion when moving (GPS course blended with IMU yaw)
• Timeout handling (GPS: 5s, IMU: 1s timeouts)
• Automatic mode switching based on data availability

Configuration

Key parameters can be adjusted in src/Fusion/GPSIMUFusion.cpp:

GPS_VELOCITY_THRESHOLD = 1.0f;  // m/s - min speed for GPS heading
GPS_TIMEOUT_MS = 5000.0f;       // GPS data timeout
IMU_TIMEOUT_MS = 1000.0f;       // IMU data timeout
FUSION_UPDATE_RATE = 50.0f;     // Hz - fusion update rate

Integration

The fusion system automatically:

1. Initializes when accelerometer thread starts
2. Updates every accelerometer cycle (~50-100 Hz)
3. Provides data through global accessor function

Files Added/Modified

New Files

• src/Fusion/GPSIMUFusion.h - Main fusion class header
• src/Fusion/GPSIMUFusion.cpp - Fusion implementation
• src/Fusion/GPSIMUFusionDemo.cpp - Usage examples and demo code
• src/Fusion/GPSIMUFusionDebug.h - Debug macros and helper functions

Modified Files

• src/motion/SensorLiveData.h - Added fusion data accessor
• src/motion/SensorLiveData.cpp - Added fusion data access function
• src/motion/AccelerometerThread.h - Added fusion initialization and updates

Performance

• CPU Usage: Minimal overhead (~1-2% additional CPU)
• Memory: ~500 bytes for fusion state
• Update Rate: 50-100 Hz (matches accelerometer thread)
• Latency: <20ms for orientation, <1s for position

Future Enhancements

Potential improvements:

1. Package IMU data into environment telemetry
2. Extended Kalman Filter for more sophisticated fusion

[CLAassistant]

All committers have signed the CLA.

[CLAassistant]

Thank you for your submission! We really appreciate it. Like many open source projects, we ask that you sign our Contributor License Agreement before we can accept your contribution.
_{You have signed the CLA already but the status is still pending? Let us recheck it.}

[MATBckh22]

Hi! This is my first PR for this project, please let me know if there's anything I should do😄