6-Axis AHRS

// EKF Version - Recommended for high precision
#include "Tactical_Fusion.h"

TacticalSystem sys;

void main() {
    // Initialize: 100Hz sample rate, 0.1Hz heave cutoff
    Tactical_Init(&sys, 100.0f, 0.1f);

    while (1) {
        // Gyroscope data: deg/s
        FusionVector gyro = {gx, gy, gz};
        // Accelerometer data: g
        FusionVector acc = {ax, ay, az};

        // Update attitude estimation
        Tactical_Update(&sys, gyro, acc);

        // Get quaternion
        FusionQuaternion q = sys.quaternion;

        // Get Euler angles
        FusionEuler euler = FusionQuaternionToEuler(q);

        // Get heave (vertical motion)
        float heave = sys.heavePosition;
    }
}

// Madgwick Version - Simple and lightweight
#include "Fusion_AHRS.h"

FusionAhrs ahrs;
FusionOffset offset;

void main() {
    FusionAhrsInitialise(&ahrs);
    FusionOffsetInitialise(&offset, 100);  // 100 Hz

    while (1) {
        FusionVector gyro = {gx, gy, gz};  // deg/s
        FusionVector acc = {ax, ay, az};   // g

        // Gyroscope bias correction
        gyro = FusionOffsetUpdate(&offset, gyro);

        // Update AHRS
        FusionAhrsUpdate, gyro, acc(&ahrs, 0.01f);

        FusionQuaternion q = FusionAhrsGetQuaternion(&ahrs);
    }
}