Use upstream fusion library (#10724)

* Use upstream fusion library * Update Fusion dependency to new repository and version
2026-07-03 22:59:18 +02:00
parent 84b1af0b83
commit babbac3308
15 changed files with 11 additions and 1657 deletions
@@ -200,6 +200,8 @@ lib_deps =
 	https://github.com/adafruit/Adafruit_TSL2561/archive/refs/tags/1.1.3.zip
 	# renovate: datasource=github-tags depName=BH1750_WE packageName=wollewald/BH1750_WE
 	https://github.com/wollewald/BH1750_WE/archive/refs/tags/1.1.10.zip
 	# renovate: datasource=git-refs depName=Fusion packageName=https://github.com/meshtastic/Fusion gitBranch=master
 	https://github.com/meshtastic/Fusion/archive/936e1eb1e5ea19e8f4ed467526f91adeebb4f53c.zip
 	# renovate: datasource=github-tags depName=Sensirion Core packageName=sensirion/arduino-core
 	https://github.com/Sensirion/arduino-core/archive/refs/tags/0.7.3.zip
 	# renovate: datasource=github-tags depName=Sensirion I2C SCD4x packageName=sensirion/arduino-i2c-scd4x
@@ -1,32 +0,0 @@
 /**
 * @file Fusion.h
 * @author Seb Madgwick
 * @brief Main header file for the Fusion library.  This is the only file that
 * needs to be included when using the library.
 */
 #ifndef FUSION_H
 #define FUSION_H
 //------------------------------------------------------------------------------
 // Includes
 #ifdef __cplusplus
 extern "C" {
 #endif
 #include "FusionAhrs.h"
 #include "FusionAxes.h"
 #include "FusionCalibration.h"
 #include "FusionCompass.h"
 #include "FusionConvention.h"
 #include "FusionMath.h"
 #include "FusionOffset.h"
 #ifdef __cplusplus
 }
 #endif
 #endif
 //------------------------------------------------------------------------------
 // End of file
@@ -1,542 +0,0 @@
 /**
 * @file FusionAhrs.c
 * @author Seb Madgwick
 * @brief AHRS algorithm to combine gyroscope, accelerometer, and magnetometer
 * measurements into a single measurement of orientation relative to the Earth.
 */
 //------------------------------------------------------------------------------
 // Includes
 #include "FusionAhrs.h"
 #include <float.h> // FLT_MAX
 #include <math.h>  // atan2f, cosf, fabsf, powf, sinf
 //------------------------------------------------------------------------------
 // Definitions
 /**
 * @brief Initial gain used during the initialisation.
 */
 #define INITIAL_GAIN (10.0f)
 /**
 * @brief Initialisation period in seconds.
 */
 #define INITIALISATION_PERIOD (3.0f)
 //------------------------------------------------------------------------------
 // Function declarations
 static inline FusionVector HalfGravity(const FusionAhrs *const ahrs);
 static inline FusionVector HalfMagnetic(const FusionAhrs *const ahrs);
 static inline FusionVector Feedback(const FusionVector sensor, const FusionVector reference);
 static inline int Clamp(const int value, const int min, const int max);
 //------------------------------------------------------------------------------
 // Functions
 /**
 * @brief Initialises the AHRS algorithm structure.
 * @param ahrs AHRS algorithm structure.
 */
 void FusionAhrsInitialise(FusionAhrs *const ahrs)
 {
    const FusionAhrsSettings settings = {
        .convention = FusionConventionNwu,
        .gain = 0.5f,
        .gyroscopeRange = 0.0f,
        .accelerationRejection = 90.0f,
        .magneticRejection = 90.0f,
        .recoveryTriggerPeriod = 0,
    };
    FusionAhrsSetSettings(ahrs, &settings);
    FusionAhrsReset(ahrs);
 }
 /**
 * @brief Resets the AHRS algorithm.  This is equivalent to reinitialising the
 * algorithm while maintaining the current settings.
 * @param ahrs AHRS algorithm structure.
 */
 void FusionAhrsReset(FusionAhrs *const ahrs)
 {
    ahrs->quaternion = FUSION_IDENTITY_QUATERNION;
    ahrs->accelerometer = FUSION_VECTOR_ZERO;
    ahrs->initialising = true;
    ahrs->rampedGain = INITIAL_GAIN;
    ahrs->angularRateRecovery = false;
    ahrs->halfAccelerometerFeedback = FUSION_VECTOR_ZERO;
    ahrs->halfMagnetometerFeedback = FUSION_VECTOR_ZERO;
    ahrs->accelerometerIgnored = false;
    ahrs->accelerationRecoveryTrigger = 0;
    ahrs->accelerationRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
    ahrs->magnetometerIgnored = false;
    ahrs->magneticRecoveryTrigger = 0;
    ahrs->magneticRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
 }
 /**
 * @brief Sets the AHRS algorithm settings.
 * @param ahrs AHRS algorithm structure.
 * @param settings Settings.
 */
 void FusionAhrsSetSettings(FusionAhrs *const ahrs, const FusionAhrsSettings *const settings)
 {
    ahrs->settings.convention = settings->convention;
    ahrs->settings.gain = settings->gain;
    ahrs->settings.gyroscopeRange = settings->gyroscopeRange == 0.0f ? FLT_MAX : 0.98f * settings->gyroscopeRange;
    ahrs->settings.accelerationRejection = settings->accelerationRejection == 0.0f
                                               ? FLT_MAX
                                               : powf(0.5f * sinf(FusionDegreesToRadians(settings->accelerationRejection)), 2);
    ahrs->settings.magneticRejection =
        settings->magneticRejection == 0.0f ? FLT_MAX : powf(0.5f * sinf(FusionDegreesToRadians(settings->magneticRejection)), 2);
    ahrs->settings.recoveryTriggerPeriod = settings->recoveryTriggerPeriod;
    ahrs->accelerationRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
    ahrs->magneticRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
    if ((settings->gain == 0.0f) ||
        (settings->recoveryTriggerPeriod == 0)) { // disable acceleration and magnetic rejection features if gain is zero
        ahrs->settings.accelerationRejection = FLT_MAX;
        ahrs->settings.magneticRejection = FLT_MAX;
    }
    if (ahrs->initialising == false) {
        ahrs->rampedGain = ahrs->settings.gain;
    }
    ahrs->rampedGainStep = (INITIAL_GAIN - ahrs->settings.gain) / INITIALISATION_PERIOD;
 }
 /**
 * @brief Updates the AHRS algorithm using the gyroscope, accelerometer, and
 * magnetometer measurements.
 * @param ahrs AHRS algorithm structure.
 * @param gyroscope Gyroscope measurement in degrees per second.
 * @param accelerometer Accelerometer measurement in g.
 * @param magnetometer Magnetometer measurement in arbitrary units.
 * @param deltaTime Delta time in seconds.
 */
 void FusionAhrsUpdate(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer,
                      const FusionVector magnetometer, const float deltaTime)
 {
 #define Q ahrs->quaternion.element
    // Store accelerometer
    ahrs->accelerometer = accelerometer;
    // Reinitialise if gyroscope range exceeded
    if ((fabsf(gyroscope.axis.x) > ahrs->settings.gyroscopeRange) || (fabsf(gyroscope.axis.y) > ahrs->settings.gyroscopeRange) ||
        (fabsf(gyroscope.axis.z) > ahrs->settings.gyroscopeRange)) {
        const FusionQuaternion quaternion = ahrs->quaternion;
        FusionAhrsReset(ahrs);
        ahrs->quaternion = quaternion;
        ahrs->angularRateRecovery = true;
    }
    // Ramp down gain during initialisation
    if (ahrs->initialising) {
        ahrs->rampedGain -= ahrs->rampedGainStep * deltaTime;
        if ((ahrs->rampedGain < ahrs->settings.gain) || (ahrs->settings.gain == 0.0f)) {
            ahrs->rampedGain = ahrs->settings.gain;
            ahrs->initialising = false;
            ahrs->angularRateRecovery = false;
        }
    }
    // Calculate direction of gravity indicated by algorithm
    const FusionVector halfGravity = HalfGravity(ahrs);
    // Calculate accelerometer feedback
    FusionVector halfAccelerometerFeedback = FUSION_VECTOR_ZERO;
    ahrs->accelerometerIgnored = true;
    if (FusionVectorIsZero(accelerometer) == false) {
        // Calculate accelerometer feedback scaled by 0.5
        ahrs->halfAccelerometerFeedback = Feedback(FusionVectorNormalise(accelerometer), halfGravity);
        // Don't ignore accelerometer if acceleration error below threshold
        if (ahrs->initialising ||
            ((FusionVectorMagnitudeSquared(ahrs->halfAccelerometerFeedback) <= ahrs->settings.accelerationRejection))) {
            ahrs->accelerometerIgnored = false;
            ahrs->accelerationRecoveryTrigger -= 9;
        } else {
            ahrs->accelerationRecoveryTrigger += 1;
        }
        // Don't ignore accelerometer during acceleration recovery
        if (ahrs->accelerationRecoveryTrigger > ahrs->accelerationRecoveryTimeout) {
            ahrs->accelerationRecoveryTimeout = 0;
            ahrs->accelerometerIgnored = false;
        } else {
            ahrs->accelerationRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
        }
        ahrs->accelerationRecoveryTrigger = Clamp(ahrs->accelerationRecoveryTrigger, 0, ahrs->settings.recoveryTriggerPeriod);
        // Apply accelerometer feedback
        if (ahrs->accelerometerIgnored == false) {
            halfAccelerometerFeedback = ahrs->halfAccelerometerFeedback;
        }
    }
    // Calculate magnetometer feedback
    FusionVector halfMagnetometerFeedback = FUSION_VECTOR_ZERO;
    ahrs->magnetometerIgnored = true;
    if (FusionVectorIsZero(magnetometer) == false) {
        // Calculate direction of magnetic field indicated by algorithm
        const FusionVector halfMagnetic = HalfMagnetic(ahrs);
        // Calculate magnetometer feedback scaled by 0.5
        ahrs->halfMagnetometerFeedback =
            Feedback(FusionVectorNormalise(FusionVectorCrossProduct(halfGravity, magnetometer)), halfMagnetic);
        // Don't ignore magnetometer if magnetic error below threshold
        if (ahrs->initialising ||
            ((FusionVectorMagnitudeSquared(ahrs->halfMagnetometerFeedback) <= ahrs->settings.magneticRejection))) {
            ahrs->magnetometerIgnored = false;
            ahrs->magneticRecoveryTrigger -= 9;
        } else {
            ahrs->magneticRecoveryTrigger += 1;
        }
        // Don't ignore magnetometer during magnetic recovery
        if (ahrs->magneticRecoveryTrigger > ahrs->magneticRecoveryTimeout) {
            ahrs->magneticRecoveryTimeout = 0;
            ahrs->magnetometerIgnored = false;
        } else {
            ahrs->magneticRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
        }
        ahrs->magneticRecoveryTrigger = Clamp(ahrs->magneticRecoveryTrigger, 0, ahrs->settings.recoveryTriggerPeriod);
        // Apply magnetometer feedback
        if (ahrs->magnetometerIgnored == false) {
            halfMagnetometerFeedback = ahrs->halfMagnetometerFeedback;
        }
    }
    // Convert gyroscope to radians per second scaled by 0.5
    const FusionVector halfGyroscope = FusionVectorMultiplyScalar(gyroscope, FusionDegreesToRadians(0.5f));
    // Apply feedback to gyroscope
    const FusionVector adjustedHalfGyroscope = FusionVectorAdd(
        halfGyroscope,
        FusionVectorMultiplyScalar(FusionVectorAdd(halfAccelerometerFeedback, halfMagnetometerFeedback), ahrs->rampedGain));
    // Integrate rate of change of quaternion
    ahrs->quaternion = FusionQuaternionAdd(
        ahrs->quaternion,
        FusionQuaternionMultiplyVector(ahrs->quaternion, FusionVectorMultiplyScalar(adjustedHalfGyroscope, deltaTime)));
    // Normalise quaternion
    ahrs->quaternion = FusionQuaternionNormalise(ahrs->quaternion);
 #undef Q
 }
 /**
 * @brief Returns the direction of gravity scaled by 0.5.
 * @param ahrs AHRS algorithm structure.
 * @return Direction of gravity scaled by 0.5.
 */
 static inline FusionVector HalfGravity(const FusionAhrs *const ahrs)
 {
 #define Q ahrs->quaternion.element
    switch (ahrs->settings.convention) {
    case FusionConventionNwu:
    case FusionConventionEnu: {
        const FusionVector halfGravity = {.axis = {
                                              .x = Q.x * Q.z - Q.w * Q.y,
                                              .y = Q.y * Q.z + Q.w * Q.x,
                                              .z = Q.w * Q.w - 0.5f + Q.z * Q.z,
                                          }}; // third column of transposed rotation matrix scaled by 0.5
        return halfGravity;
    }
    case FusionConventionNed: {
        const FusionVector halfGravity = {.axis = {
                                              .x = Q.w * Q.y - Q.x * Q.z,
                                              .y = -1.0f * (Q.y * Q.z + Q.w * Q.x),
                                              .z = 0.5f - Q.w * Q.w - Q.z * Q.z,
                                          }}; // third column of transposed rotation matrix scaled by -0.5
        return halfGravity;
    }
    }
    return FUSION_VECTOR_ZERO; // avoid compiler warning
 #undef Q
 }
 /**
 * @brief Returns the direction of the magnetic field scaled by 0.5.
 * @param ahrs AHRS algorithm structure.
 * @return Direction of the magnetic field scaled by 0.5.
 */
 static inline FusionVector HalfMagnetic(const FusionAhrs *const ahrs)
 {
 #define Q ahrs->quaternion.element
    switch (ahrs->settings.convention) {
    case FusionConventionNwu: {
        const FusionVector halfMagnetic = {.axis = {
                                               .x = Q.x * Q.y + Q.w * Q.z,
                                               .y = Q.w * Q.w - 0.5f + Q.y * Q.y,
                                               .z = Q.y * Q.z - Q.w * Q.x,
                                           }}; // second column of transposed rotation matrix scaled by 0.5
        return halfMagnetic;
    }
    case FusionConventionEnu: {
        const FusionVector halfMagnetic = {.axis = {
                                               .x = 0.5f - Q.w * Q.w - Q.x * Q.x,
                                               .y = Q.w * Q.z - Q.x * Q.y,
                                               .z = -1.0f * (Q.x * Q.z + Q.w * Q.y),
                                           }}; // first column of transposed rotation matrix scaled by -0.5
        return halfMagnetic;
    }
    case FusionConventionNed: {
        const FusionVector halfMagnetic = {.axis = {
                                               .x = -1.0f * (Q.x * Q.y + Q.w * Q.z),
                                               .y = 0.5f - Q.w * Q.w - Q.y * Q.y,
                                               .z = Q.w * Q.x - Q.y * Q.z,
                                           }}; // second column of transposed rotation matrix scaled by -0.5
        return halfMagnetic;
    }
    }
    return FUSION_VECTOR_ZERO; // avoid compiler warning
 #undef Q
 }
 /**
 * @brief Returns the feedback.
 * @param sensor Sensor.
 * @param reference Reference.
 * @return Feedback.
 */
 static inline FusionVector Feedback(const FusionVector sensor, const FusionVector reference)
 {
    if (FusionVectorDotProduct(sensor, reference) < 0.0f) { // if error is >90 degrees
        return FusionVectorNormalise(FusionVectorCrossProduct(sensor, reference));
    }
    return FusionVectorCrossProduct(sensor, reference);
 }
 /**
 * @brief Returns a value limited to maximum and minimum.
 * @param value Value.
 * @param min Minimum value.
 * @param max Maximum value.
 * @return Value limited to maximum and minimum.
 */
 static inline int Clamp(const int value, const int min, const int max)
 {
    if (value < min) {
        return min;
    }
    if (value > max) {
        return max;
    }
    return value;
 }
 /**
 * @brief Updates the AHRS algorithm using the gyroscope and accelerometer
 * measurements only.
 * @param ahrs AHRS algorithm structure.
 * @param gyroscope Gyroscope measurement in degrees per second.
 * @param accelerometer Accelerometer measurement in g.
 * @param deltaTime Delta time in seconds.
 */
 void FusionAhrsUpdateNoMagnetometer(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer,
                                    const float deltaTime)
 {
    // Update AHRS algorithm
    FusionAhrsUpdate(ahrs, gyroscope, accelerometer, FUSION_VECTOR_ZERO, deltaTime);
    // Zero heading during initialisation
    if (ahrs->initialising) {
        FusionAhrsSetHeading(ahrs, 0.0f);
    }
 }
 /**
 * @brief Updates the AHRS algorithm using the gyroscope, accelerometer, and
 * heading measurements.
 * @param ahrs AHRS algorithm structure.
 * @param gyroscope Gyroscope measurement in degrees per second.
 * @param accelerometer Accelerometer measurement in g.
 * @param heading Heading measurement in degrees.
 * @param deltaTime Delta time in seconds.
 */
 void FusionAhrsUpdateExternalHeading(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer,
                                     const float heading, const float deltaTime)
 {
 #define Q ahrs->quaternion.element
    // Calculate roll
    const float roll = atan2f(Q.w * Q.x + Q.y * Q.z, 0.5f - Q.y * Q.y - Q.x * Q.x);
    // Calculate magnetometer
    const float headingRadians = FusionDegreesToRadians(heading);
    const float sinHeadingRadians = sinf(headingRadians);
    const FusionVector magnetometer = {.axis = {
                                           .x = cosf(headingRadians),
                                           .y = -1.0f * cosf(roll) * sinHeadingRadians,
                                           .z = sinHeadingRadians * sinf(roll),
                                       }};
    // Update AHRS algorithm
    FusionAhrsUpdate(ahrs, gyroscope, accelerometer, magnetometer, deltaTime);
 #undef Q
 }
 /**
 * @brief Returns the quaternion describing the sensor relative to the Earth.
 * @param ahrs AHRS algorithm structure.
 * @return Quaternion describing the sensor relative to the Earth.
 */
 FusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs *const ahrs)
 {
    return ahrs->quaternion;
 }
 /**
 * @brief Sets the quaternion describing the sensor relative to the Earth.
 * @param ahrs AHRS algorithm structure.
 * @param quaternion Quaternion describing the sensor relative to the Earth.
 */
 void FusionAhrsSetQuaternion(FusionAhrs *const ahrs, const FusionQuaternion quaternion)
 {
    ahrs->quaternion = quaternion;
 }
 /**
 * @brief Returns the linear acceleration measurement equal to the accelerometer
 * measurement with the 1 g of gravity removed.
 * @param ahrs AHRS algorithm structure.
 * @return Linear acceleration measurement in g.
 */
 FusionVector FusionAhrsGetLinearAcceleration(const FusionAhrs *const ahrs)
 {
 #define Q ahrs->quaternion.element
    // Calculate gravity in the sensor coordinate frame
    const FusionVector gravity = {.axis = {
                                      .x = 2.0f * (Q.x * Q.z - Q.w * Q.y),
                                      .y = 2.0f * (Q.y * Q.z + Q.w * Q.x),
                                      .z = 2.0f * (Q.w * Q.w - 0.5f + Q.z * Q.z),
                                  }}; // third column of transposed rotation matrix
    // Remove gravity from accelerometer measurement
    switch (ahrs->settings.convention) {
    case FusionConventionNwu:
    case FusionConventionEnu: {
        return FusionVectorSubtract(ahrs->accelerometer, gravity);
    }
    case FusionConventionNed: {
        return FusionVectorAdd(ahrs->accelerometer, gravity);
    }
    }
    return FUSION_VECTOR_ZERO; // avoid compiler warning
 #undef Q
 }
 /**
 * @brief Returns the Earth acceleration measurement equal to accelerometer
 * measurement in the Earth coordinate frame with the 1 g of gravity removed.
 * @param ahrs AHRS algorithm structure.
 * @return Earth acceleration measurement in g.
 */
 FusionVector FusionAhrsGetEarthAcceleration(const FusionAhrs *const ahrs)
 {
 #define Q ahrs->quaternion.element
 #define A ahrs->accelerometer.axis
    // Calculate accelerometer measurement in the Earth coordinate frame
    const float qwqw = Q.w * Q.w; // calculate common terms to avoid repeated operations
    const float qwqx = Q.w * Q.x;
    const float qwqy = Q.w * Q.y;
    const float qwqz = Q.w * Q.z;
    const float qxqy = Q.x * Q.y;
    const float qxqz = Q.x * Q.z;
    const float qyqz = Q.y * Q.z;
    FusionVector accelerometer = {.axis = {
                                      .x = 2.0f * ((qwqw - 0.5f + Q.x * Q.x) * A.x + (qxqy - qwqz) * A.y + (qxqz + qwqy) * A.z),
                                      .y = 2.0f * ((qxqy + qwqz) * A.x + (qwqw - 0.5f + Q.y * Q.y) * A.y + (qyqz - qwqx) * A.z),
                                      .z = 2.0f * ((qxqz - qwqy) * A.x + (qyqz + qwqx) * A.y + (qwqw - 0.5f + Q.z * Q.z) * A.z),
                                  }}; // rotation matrix multiplied with the accelerometer
    // Remove gravity from accelerometer measurement
    switch (ahrs->settings.convention) {
    case FusionConventionNwu:
    case FusionConventionEnu:
        accelerometer.axis.z -= 1.0f;
        break;
    case FusionConventionNed:
        accelerometer.axis.z += 1.0f;
        break;
    }
    return accelerometer;
 #undef Q
 #undef A
 }
 /**
 * @brief Returns the AHRS algorithm internal states.
 * @param ahrs AHRS algorithm structure.
 * @return AHRS algorithm internal states.
 */
 FusionAhrsInternalStates FusionAhrsGetInternalStates(const FusionAhrs *const ahrs)
 {
    const FusionAhrsInternalStates internalStates = {
        .accelerationError = FusionRadiansToDegrees(FusionAsin(2.0f * FusionVectorMagnitude(ahrs->halfAccelerometerFeedback))),
        .accelerometerIgnored = ahrs->accelerometerIgnored,
        .accelerationRecoveryTrigger =
            ahrs->settings.recoveryTriggerPeriod == 0
                ? 0.0f
                : (float)ahrs->accelerationRecoveryTrigger / (float)ahrs->settings.recoveryTriggerPeriod,
        .magneticError = FusionRadiansToDegrees(FusionAsin(2.0f * FusionVectorMagnitude(ahrs->halfMagnetometerFeedback))),
        .magnetometerIgnored = ahrs->magnetometerIgnored,
        .magneticRecoveryTrigger = ahrs->settings.recoveryTriggerPeriod == 0
                                       ? 0.0f
                                       : (float)ahrs->magneticRecoveryTrigger / (float)ahrs->settings.recoveryTriggerPeriod,
    };
    return internalStates;
 }
 /**
 * @brief Returns the AHRS algorithm flags.
 * @param ahrs AHRS algorithm structure.
 * @return AHRS algorithm flags.
 */
 FusionAhrsFlags FusionAhrsGetFlags(const FusionAhrs *const ahrs)
 {
    const FusionAhrsFlags flags = {
        .initialising = ahrs->initialising,
        .angularRateRecovery = ahrs->angularRateRecovery,
        .accelerationRecovery = ahrs->accelerationRecoveryTrigger > ahrs->accelerationRecoveryTimeout,
        .magneticRecovery = ahrs->magneticRecoveryTrigger > ahrs->magneticRecoveryTimeout,
    };
    return flags;
 }
 /**
 * @brief Sets the heading of the orientation measurement provided by the AHRS
 * algorithm.  This function can be used to reset drift in heading when the AHRS
 * algorithm is being used without a magnetometer.
 * @param ahrs AHRS algorithm structure.
 * @param heading Heading angle in degrees.
 */
 void FusionAhrsSetHeading(FusionAhrs *const ahrs, const float heading)
 {
 #define Q ahrs->quaternion.element
    const float yaw = atan2f(Q.w * Q.z + Q.x * Q.y, 0.5f - Q.y * Q.y - Q.z * Q.z);
    const float halfYawMinusHeading = 0.5f * (yaw - FusionDegreesToRadians(heading));
    const FusionQuaternion rotation = {.element = {
                                           .w = cosf(halfYawMinusHeading),
                                           .x = 0.0f,
                                           .y = 0.0f,
                                           .z = -1.0f * sinf(halfYawMinusHeading),
                                       }};
    ahrs->quaternion = FusionQuaternionMultiply(rotation, ahrs->quaternion);
 #undef Q
 }
 //------------------------------------------------------------------------------
 // End of file
@@ -1,112 +0,0 @@
 /**
 * @file FusionAhrs.h
 * @author Seb Madgwick
 * @brief AHRS algorithm to combine gyroscope, accelerometer, and magnetometer
 * measurements into a single measurement of orientation relative to the Earth.
 */
 #ifndef FUSION_AHRS_H
 #define FUSION_AHRS_H
 //------------------------------------------------------------------------------
 // Includes
 #include "FusionConvention.h"
 #include "FusionMath.h"
 #include <stdbool.h>
 //------------------------------------------------------------------------------
 // Definitions
 /**
 * @brief AHRS algorithm settings.
 */
 typedef struct {
    FusionConvention convention;
    float gain;
    float gyroscopeRange;
    float accelerationRejection;
    float magneticRejection;
    unsigned int recoveryTriggerPeriod;
 } FusionAhrsSettings;
 /**
 * @brief AHRS algorithm structure.  Structure members are used internally and
 * must not be accessed by the application.
 */
 typedef struct {
    FusionAhrsSettings settings;
    FusionQuaternion quaternion;
    FusionVector accelerometer;
    bool initialising;
    float rampedGain;
    float rampedGainStep;
    bool angularRateRecovery;
    FusionVector halfAccelerometerFeedback;
    FusionVector halfMagnetometerFeedback;
    bool accelerometerIgnored;
    int accelerationRecoveryTrigger;
    int accelerationRecoveryTimeout;
    bool magnetometerIgnored;
    int magneticRecoveryTrigger;
    int magneticRecoveryTimeout;
 } FusionAhrs;
 /**
 * @brief AHRS algorithm internal states.
 */
 typedef struct {
    float accelerationError;
    bool accelerometerIgnored;
    float accelerationRecoveryTrigger;
    float magneticError;
    bool magnetometerIgnored;
    float magneticRecoveryTrigger;
 } FusionAhrsInternalStates;
 /**
 * @brief AHRS algorithm flags.
 */
 typedef struct {
    bool initialising;
    bool angularRateRecovery;
    bool accelerationRecovery;
    bool magneticRecovery;
 } FusionAhrsFlags;
 //------------------------------------------------------------------------------
 // Function declarations
 void FusionAhrsInitialise(FusionAhrs *const ahrs);
 void FusionAhrsReset(FusionAhrs *const ahrs);
 void FusionAhrsSetSettings(FusionAhrs *const ahrs, const FusionAhrsSettings *const settings);
 void FusionAhrsUpdate(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer,
                      const FusionVector magnetometer, const float deltaTime);
 void FusionAhrsUpdateNoMagnetometer(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer,
                                    const float deltaTime);
 void FusionAhrsUpdateExternalHeading(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer,
                                     const float heading, const float deltaTime);
 FusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs *const ahrs);
 void FusionAhrsSetQuaternion(FusionAhrs *const ahrs, const FusionQuaternion quaternion);
 FusionVector FusionAhrsGetLinearAcceleration(const FusionAhrs *const ahrs);
 FusionVector FusionAhrsGetEarthAcceleration(const FusionAhrs *const ahrs);
 FusionAhrsInternalStates FusionAhrsGetInternalStates(const FusionAhrs *const ahrs);
 FusionAhrsFlags FusionAhrsGetFlags(const FusionAhrs *const ahrs);
 void FusionAhrsSetHeading(FusionAhrs *const ahrs, const float heading);
 #endif
 //------------------------------------------------------------------------------
 // End of file
@@ -1,188 +0,0 @@
 /**
 * @file FusionAxes.h
 * @author Seb Madgwick
 * @brief Swaps sensor axes for alignment with the body axes.
 */
 #ifndef FUSION_AXES_H
 #define FUSION_AXES_H
 //------------------------------------------------------------------------------
 // Includes
 #include "FusionMath.h"
 //------------------------------------------------------------------------------
 // Definitions
 /**
 * @brief Axes alignment describing the sensor axes relative to the body axes.
 * For example, if the body X axis is aligned with the sensor Y axis and the
 * body Y axis is aligned with sensor X axis but pointing the opposite direction
 * then alignment is +Y-X+Z.
 */
 typedef enum {
    FusionAxesAlignmentPXPYPZ, /* +X+Y+Z */
    FusionAxesAlignmentPXNZPY, /* +X-Z+Y */
    FusionAxesAlignmentPXNYNZ, /* +X-Y-Z */
    FusionAxesAlignmentPXPZNY, /* +X+Z-Y */
    FusionAxesAlignmentNXPYNZ, /* -X+Y-Z */
    FusionAxesAlignmentNXPZPY, /* -X+Z+Y */
    FusionAxesAlignmentNXNYPZ, /* -X-Y+Z */
    FusionAxesAlignmentNXNZNY, /* -X-Z-Y */
    FusionAxesAlignmentPYNXPZ, /* +Y-X+Z */
    FusionAxesAlignmentPYNZNX, /* +Y-Z-X */
    FusionAxesAlignmentPYPXNZ, /* +Y+X-Z */
    FusionAxesAlignmentPYPZPX, /* +Y+Z+X */
    FusionAxesAlignmentNYPXPZ, /* -Y+X+Z */
    FusionAxesAlignmentNYNZPX, /* -Y-Z+X */
    FusionAxesAlignmentNYNXNZ, /* -Y-X-Z */
    FusionAxesAlignmentNYPZNX, /* -Y+Z-X */
    FusionAxesAlignmentPZPYNX, /* +Z+Y-X */
    FusionAxesAlignmentPZPXPY, /* +Z+X+Y */
    FusionAxesAlignmentPZNYPX, /* +Z-Y+X */
    FusionAxesAlignmentPZNXNY, /* +Z-X-Y */
    FusionAxesAlignmentNZPYPX, /* -Z+Y+X */
    FusionAxesAlignmentNZNXPY, /* -Z-X+Y */
    FusionAxesAlignmentNZNYNX, /* -Z-Y-X */
    FusionAxesAlignmentNZPXNY, /* -Z+X-Y */
 } FusionAxesAlignment;
 //------------------------------------------------------------------------------
 // Inline functions
 /**
 * @brief Swaps sensor axes for alignment with the body axes.
 * @param sensor Sensor axes.
 * @param alignment Axes alignment.
 * @return Sensor axes aligned with the body axes.
 */
 static inline FusionVector FusionAxesSwap(const FusionVector sensor, const FusionAxesAlignment alignment)
 {
    FusionVector result;
    switch (alignment) {
    case FusionAxesAlignmentPXPYPZ:
        break;
    case FusionAxesAlignmentPXNZPY:
        result.axis.x = +sensor.axis.x;
        result.axis.y = -sensor.axis.z;
        result.axis.z = +sensor.axis.y;
        return result;
    case FusionAxesAlignmentPXNYNZ:
        result.axis.x = +sensor.axis.x;
        result.axis.y = -sensor.axis.y;
        result.axis.z = -sensor.axis.z;
        return result;
    case FusionAxesAlignmentPXPZNY:
        result.axis.x = +sensor.axis.x;
        result.axis.y = +sensor.axis.z;
        result.axis.z = -sensor.axis.y;
        return result;
    case FusionAxesAlignmentNXPYNZ:
        result.axis.x = -sensor.axis.x;
        result.axis.y = +sensor.axis.y;
        result.axis.z = -sensor.axis.z;
        return result;
    case FusionAxesAlignmentNXPZPY:
        result.axis.x = -sensor.axis.x;
        result.axis.y = +sensor.axis.z;
        result.axis.z = +sensor.axis.y;
        return result;
    case FusionAxesAlignmentNXNYPZ:
        result.axis.x = -sensor.axis.x;
        result.axis.y = -sensor.axis.y;
        result.axis.z = +sensor.axis.z;
        return result;
    case FusionAxesAlignmentNXNZNY:
        result.axis.x = -sensor.axis.x;
        result.axis.y = -sensor.axis.z;
        result.axis.z = -sensor.axis.y;
        return result;
    case FusionAxesAlignmentPYNXPZ:
        result.axis.x = +sensor.axis.y;
        result.axis.y = -sensor.axis.x;
        result.axis.z = +sensor.axis.z;
        return result;
    case FusionAxesAlignmentPYNZNX:
        result.axis.x = +sensor.axis.y;
        result.axis.y = -sensor.axis.z;
        result.axis.z = -sensor.axis.x;
        return result;
    case FusionAxesAlignmentPYPXNZ:
        result.axis.x = +sensor.axis.y;
        result.axis.y = +sensor.axis.x;
        result.axis.z = -sensor.axis.z;
        return result;
    case FusionAxesAlignmentPYPZPX:
        result.axis.x = +sensor.axis.y;
        result.axis.y = +sensor.axis.z;
        result.axis.z = +sensor.axis.x;
        return result;
    case FusionAxesAlignmentNYPXPZ:
        result.axis.x = -sensor.axis.y;
        result.axis.y = +sensor.axis.x;
        result.axis.z = +sensor.axis.z;
        return result;
    case FusionAxesAlignmentNYNZPX:
        result.axis.x = -sensor.axis.y;
        result.axis.y = -sensor.axis.z;
        result.axis.z = +sensor.axis.x;
        return result;
    case FusionAxesAlignmentNYNXNZ:
        result.axis.x = -sensor.axis.y;
        result.axis.y = -sensor.axis.x;
        result.axis.z = -sensor.axis.z;
        return result;
    case FusionAxesAlignmentNYPZNX:
        result.axis.x = -sensor.axis.y;
        result.axis.y = +sensor.axis.z;
        result.axis.z = -sensor.axis.x;
        return result;
    case FusionAxesAlignmentPZPYNX:
        result.axis.x = +sensor.axis.z;
        result.axis.y = +sensor.axis.y;
        result.axis.z = -sensor.axis.x;
        return result;
    case FusionAxesAlignmentPZPXPY:
        result.axis.x = +sensor.axis.z;
        result.axis.y = +sensor.axis.x;
        result.axis.z = +sensor.axis.y;
        return result;
    case FusionAxesAlignmentPZNYPX:
        result.axis.x = +sensor.axis.z;
        result.axis.y = -sensor.axis.y;
        result.axis.z = +sensor.axis.x;
        return result;
    case FusionAxesAlignmentPZNXNY:
        result.axis.x = +sensor.axis.z;
        result.axis.y = -sensor.axis.x;
        result.axis.z = -sensor.axis.y;
        return result;
    case FusionAxesAlignmentNZPYPX:
        result.axis.x = -sensor.axis.z;
        result.axis.y = +sensor.axis.y;
        result.axis.z = +sensor.axis.x;
        return result;
    case FusionAxesAlignmentNZNXPY:
        result.axis.x = -sensor.axis.z;
        result.axis.y = -sensor.axis.x;
        result.axis.z = +sensor.axis.y;
        return result;
    case FusionAxesAlignmentNZNYNX:
        result.axis.x = -sensor.axis.z;
        result.axis.y = -sensor.axis.y;
        result.axis.z = -sensor.axis.x;
        return result;
    case FusionAxesAlignmentNZPXNY:
        result.axis.x = -sensor.axis.z;
        result.axis.y = +sensor.axis.x;
        result.axis.z = -sensor.axis.y;
        return result;
    }
    return sensor; // avoid compiler warning
 }
 #endif
 //------------------------------------------------------------------------------
 // End of file
@@ -1,49 +0,0 @@
 /**
 * @file FusionCalibration.h
 * @author Seb Madgwick
 * @brief Gyroscope, accelerometer, and magnetometer calibration models.
 */
 #ifndef FUSION_CALIBRATION_H
 #define FUSION_CALIBRATION_H
 //------------------------------------------------------------------------------
 // Includes
 #include "FusionMath.h"
 //------------------------------------------------------------------------------
 // Inline functions
 /**
 * @brief Gyroscope and accelerometer calibration model.
 * @param uncalibrated Uncalibrated measurement.
 * @param misalignment Misalignment matrix.
 * @param sensitivity Sensitivity.
 * @param offset Offset.
 * @return Calibrated measurement.
 */
 static inline FusionVector FusionCalibrationInertial(const FusionVector uncalibrated, const FusionMatrix misalignment,
                                                     const FusionVector sensitivity, const FusionVector offset)
 {
    return FusionMatrixMultiplyVector(misalignment,
                                      FusionVectorHadamardProduct(FusionVectorSubtract(uncalibrated, offset), sensitivity));
 }
 /**
 * @brief Magnetometer calibration model.
 * @param uncalibrated Uncalibrated measurement.
 * @param softIronMatrix Soft-iron matrix.
 * @param hardIronOffset Hard-iron offset.
 * @return Calibrated measurement.
 */
 static inline FusionVector FusionCalibrationMagnetic(const FusionVector uncalibrated, const FusionMatrix softIronMatrix,
                                                     const FusionVector hardIronOffset)
 {
    return FusionMatrixMultiplyVector(softIronMatrix, FusionVectorSubtract(uncalibrated, hardIronOffset));
 }
 #endif
 //------------------------------------------------------------------------------
 // End of file
@@ -1,51 +0,0 @@
 /**
 * @file FusionCompass.c
 * @author Seb Madgwick
 * @brief Tilt-compensated compass to calculate the magnetic heading using
 * accelerometer and magnetometer measurements.
 */
 //------------------------------------------------------------------------------
 // Includes
 #include "FusionCompass.h"
 #include "FusionAxes.h"
 #include <math.h> // atan2f
 //------------------------------------------------------------------------------
 // Functions
 /**
 * @brief Calculates the magnetic heading.
 * @param convention Earth axes convention.
 * @param accelerometer Accelerometer measurement in any calibrated units.
 * @param magnetometer Magnetometer measurement in any calibrated units.
 * @return Heading angle in degrees.
 */
 float FusionCompassCalculateHeading(const FusionConvention convention, const FusionVector accelerometer,
                                    const FusionVector magnetometer)
 {
    switch (convention) {
    case FusionConventionNwu: {
        const FusionVector west = FusionVectorNormalise(FusionVectorCrossProduct(accelerometer, magnetometer));
        const FusionVector north = FusionVectorNormalise(FusionVectorCrossProduct(west, accelerometer));
        return FusionRadiansToDegrees(atan2f(west.axis.x, north.axis.x));
    }
    case FusionConventionEnu: {
        const FusionVector west = FusionVectorNormalise(FusionVectorCrossProduct(accelerometer, magnetometer));
        const FusionVector north = FusionVectorNormalise(FusionVectorCrossProduct(west, accelerometer));
        const FusionVector east = FusionVectorMultiplyScalar(west, -1.0f);
        return FusionRadiansToDegrees(atan2f(north.axis.x, east.axis.x));
    }
    case FusionConventionNed: {
        const FusionVector up = FusionVectorMultiplyScalar(accelerometer, -1.0f);
        const FusionVector west = FusionVectorNormalise(FusionVectorCrossProduct(up, magnetometer));
        const FusionVector north = FusionVectorNormalise(FusionVectorCrossProduct(west, up));
        return FusionRadiansToDegrees(atan2f(west.axis.x, north.axis.x));
    }
    }
    return 0; // avoid compiler warning
 }
 //------------------------------------------------------------------------------
 // End of file
@@ -1,26 +0,0 @@
 /**
 * @file FusionCompass.h
 * @author Seb Madgwick
 * @brief Tilt-compensated compass to calculate the magnetic heading using
 * accelerometer and magnetometer measurements.
 */
 #ifndef FUSION_COMPASS_H
 #define FUSION_COMPASS_H
 //------------------------------------------------------------------------------
 // Includes
 #include "FusionConvention.h"
 #include "FusionMath.h"
 //------------------------------------------------------------------------------
 // Function declarations
 float FusionCompassCalculateHeading(const FusionConvention convention, const FusionVector accelerometer,
                                    const FusionVector magnetometer);
 #endif
 //------------------------------------------------------------------------------
 // End of file
@@ -1,25 +0,0 @@
 /**
 * @file FusionConvention.h
 * @author Seb Madgwick
 * @brief Earth axes convention.
 */
 #ifndef FUSION_CONVENTION_H
 #define FUSION_CONVENTION_H
 //------------------------------------------------------------------------------
 // Definitions
 /**
 * @brief Earth axes convention.
 */
 typedef enum {
    FusionConventionNwu, /* North-West-Up */
    FusionConventionEnu, /* East-North-Up */
    FusionConventionNed, /* North-East-Down */
 } FusionConvention;
 #endif
 //------------------------------------------------------------------------------
 // End of file
@@ -1,503 +0,0 @@
 /**
 * @file FusionMath.h
 * @author Seb Madgwick
 * @brief Math library.
 */
 #ifndef FUSION_MATH_H
 #define FUSION_MATH_H
 //------------------------------------------------------------------------------
 // Includes
 #include <math.h> // M_PI, sqrtf, atan2f, asinf
 #include <stdbool.h>
 #include <stdint.h>
 //------------------------------------------------------------------------------
 // Definitions
 /**
 * @brief 3D vector.
 */
 typedef union {
    float array[3];
    struct {
        float x;
        float y;
        float z;
    } axis;
 } FusionVector;
 /**
 * @brief Quaternion.
 */
 typedef union {
    float array[4];
    struct {
        float w;
        float x;
        float y;
        float z;
    } element;
 } FusionQuaternion;
 /**
 * @brief 3x3 matrix in row-major order.
 * See http://en.wikipedia.org/wiki/Row-major_order
 */
 typedef union {
    float array[3][3];
    struct {
        float xx;
        float xy;
        float xz;
        float yx;
        float yy;
        float yz;
        float zx;
        float zy;
        float zz;
    } element;
 } FusionMatrix;
 /**
 * @brief Euler angles.  Roll, pitch, and yaw correspond to rotations around
 * X, Y, and Z respectively.
 */
 typedef union {
    float array[3];
    struct {
        float roll;
        float pitch;
        float yaw;
    } angle;
 } FusionEuler;
 /**
 * @brief Vector of zeros.
 */
 #define FUSION_VECTOR_ZERO ((FusionVector){.array = {0.0f, 0.0f, 0.0f}})
 /**
 * @brief Vector of ones.
 */
 #define FUSION_VECTOR_ONES ((FusionVector){.array = {1.0f, 1.0f, 1.0f}})
 /**
 * @brief Identity quaternion.
 */
 #define FUSION_IDENTITY_QUATERNION ((FusionQuaternion){.array = {1.0f, 0.0f, 0.0f, 0.0f}})
 /**
 * @brief Identity matrix.
 */
 #define FUSION_IDENTITY_MATRIX ((FusionMatrix){.array = {{1.0f, 0.0f, 0.0f}, {0.0f, 1.0f, 0.0f}, {0.0f, 0.0f, 1.0f}}})
 /**
 * @brief Euler angles of zero.
 */
 #define FUSION_EULER_ZERO ((FusionEuler){.array = {0.0f, 0.0f, 0.0f}})
 /**
 * @brief Pi. May not be defined in math.h.
 */
 #ifndef M_PI
 #define M_PI (3.14159265358979323846)
 #endif
 /**
 * @brief Include this definition or add as a preprocessor definition to use
 * normal square root operations.
 */
 // #define FUSION_USE_NORMAL_SQRT
 //------------------------------------------------------------------------------
 // Inline functions - Degrees and radians conversion
 /**
 * @brief Converts degrees to radians.
 * @param degrees Degrees.
 * @return Radians.
 */
 static inline float FusionDegreesToRadians(const float degrees)
 {
    return degrees * ((float)M_PI / 180.0f);
 }
 /**
 * @brief Converts radians to degrees.
 * @param radians Radians.
 * @return Degrees.
 */
 static inline float FusionRadiansToDegrees(const float radians)
 {
    return radians * (180.0f / (float)M_PI);
 }
 //------------------------------------------------------------------------------
 // Inline functions - Arc sine
 /**
 * @brief Returns the arc sine of the value.
 * @param value Value.
 * @return Arc sine of the value.
 */
 static inline float FusionAsin(const float value)
 {
    if (value <= -1.0f) {
        return (float)M_PI / -2.0f;
    }
    if (value >= 1.0f) {
        return (float)M_PI / 2.0f;
    }
    return asinf(value);
 }
 //------------------------------------------------------------------------------
 // Inline functions - Fast inverse square root
 #ifndef FUSION_USE_NORMAL_SQRT
 /**
 * @brief Calculates the reciprocal of the square root.
 * See https://pizer.wordpress.com/2008/10/12/fast-inverse-square-root/
 * @param x Operand.
 * @return Reciprocal of the square root of x.
 */
 static inline float FusionFastInverseSqrt(const float x)
 {
    typedef union {
        float f;
        int32_t i;
    } Union32;
    Union32 union32 = {.f = x};
    union32.i = 0x5F1F1412 - (union32.i >> 1);
    return union32.f * (1.69000231f - 0.714158168f * x * union32.f * union32.f);
 }
 #endif
 //------------------------------------------------------------------------------
 // Inline functions - Vector operations
 /**
 * @brief Returns true if the vector is zero.
 * @param vector Vector.
 * @return True if the vector is zero.
 */
 static inline bool FusionVectorIsZero(const FusionVector vector)
 {
    return (vector.axis.x == 0.0f) && (vector.axis.y == 0.0f) && (vector.axis.z == 0.0f);
 }
 /**
 * @brief Returns the sum of two vectors.
 * @param vectorA Vector A.
 * @param vectorB Vector B.
 * @return Sum of two vectors.
 */
 static inline FusionVector FusionVectorAdd(const FusionVector vectorA, const FusionVector vectorB)
 {
    const FusionVector result = {.axis = {
                                     .x = vectorA.axis.x + vectorB.axis.x,
                                     .y = vectorA.axis.y + vectorB.axis.y,
                                     .z = vectorA.axis.z + vectorB.axis.z,
                                 }};
    return result;
 }
 /**
 * @brief Returns vector B subtracted from vector A.
 * @param vectorA Vector A.
 * @param vectorB Vector B.
 * @return Vector B subtracted from vector A.
 */
 static inline FusionVector FusionVectorSubtract(const FusionVector vectorA, const FusionVector vectorB)
 {
    const FusionVector result = {.axis = {
                                     .x = vectorA.axis.x - vectorB.axis.x,
                                     .y = vectorA.axis.y - vectorB.axis.y,
                                     .z = vectorA.axis.z - vectorB.axis.z,
                                 }};
    return result;
 }
 /**
 * @brief Returns the sum of the elements.
 * @param vector Vector.
 * @return Sum of the elements.
 */
 static inline float FusionVectorSum(const FusionVector vector)
 {
    return vector.axis.x + vector.axis.y + vector.axis.z;
 }
 /**
 * @brief Returns the multiplication of a vector by a scalar.
 * @param vector Vector.
 * @param scalar Scalar.
 * @return Multiplication of a vector by a scalar.
 */
 static inline FusionVector FusionVectorMultiplyScalar(const FusionVector vector, const float scalar)
 {
    const FusionVector result = {.axis = {
                                     .x = vector.axis.x * scalar,
                                     .y = vector.axis.y * scalar,
                                     .z = vector.axis.z * scalar,
                                 }};
    return result;
 }
 /**
 * @brief Calculates the Hadamard product (element-wise multiplication).
 * @param vectorA Vector A.
 * @param vectorB Vector B.
 * @return Hadamard product.
 */
 static inline FusionVector FusionVectorHadamardProduct(const FusionVector vectorA, const FusionVector vectorB)
 {
    const FusionVector result = {.axis = {
                                     .x = vectorA.axis.x * vectorB.axis.x,
                                     .y = vectorA.axis.y * vectorB.axis.y,
                                     .z = vectorA.axis.z * vectorB.axis.z,
                                 }};
    return result;
 }
 /**
 * @brief Returns the cross product.
 * @param vectorA Vector A.
 * @param vectorB Vector B.
 * @return Cross product.
 */
 static inline FusionVector FusionVectorCrossProduct(const FusionVector vectorA, const FusionVector vectorB)
 {
 #define A vectorA.axis
 #define B vectorB.axis
    const FusionVector result = {.axis = {
                                     .x = A.y * B.z - A.z * B.y,
                                     .y = A.z * B.x - A.x * B.z,
                                     .z = A.x * B.y - A.y * B.x,
                                 }};
    return result;
 #undef A
 #undef B
 }
 /**
 * @brief Returns the dot product.
 * @param vectorA Vector A.
 * @param vectorB Vector B.
 * @return Dot product.
 */
 static inline float FusionVectorDotProduct(const FusionVector vectorA, const FusionVector vectorB)
 {
    return FusionVectorSum(FusionVectorHadamardProduct(vectorA, vectorB));
 }
 /**
 * @brief Returns the vector magnitude squared.
 * @param vector Vector.
 * @return Vector magnitude squared.
 */
 static inline float FusionVectorMagnitudeSquared(const FusionVector vector)
 {
    return FusionVectorSum(FusionVectorHadamardProduct(vector, vector));
 }
 /**
 * @brief Returns the vector magnitude.
 * @param vector Vector.
 * @return Vector magnitude.
 */
 static inline float FusionVectorMagnitude(const FusionVector vector)
 {
    return sqrtf(FusionVectorMagnitudeSquared(vector));
 }
 /**
 * @brief Returns the normalised vector.
 * @param vector Vector.
 * @return Normalised vector.
 */
 static inline FusionVector FusionVectorNormalise(const FusionVector vector)
 {
 #ifdef FUSION_USE_NORMAL_SQRT
    const float magnitudeReciprocal = 1.0f / sqrtf(FusionVectorMagnitudeSquared(vector));
 #else
    const float magnitudeReciprocal = FusionFastInverseSqrt(FusionVectorMagnitudeSquared(vector));
 #endif
    return FusionVectorMultiplyScalar(vector, magnitudeReciprocal);
 }
 //------------------------------------------------------------------------------
 // Inline functions - Quaternion operations
 /**
 * @brief Returns the sum of two quaternions.
 * @param quaternionA Quaternion A.
 * @param quaternionB Quaternion B.
 * @return Sum of two quaternions.
 */
 static inline FusionQuaternion FusionQuaternionAdd(const FusionQuaternion quaternionA, const FusionQuaternion quaternionB)
 {
    const FusionQuaternion result = {.element = {
                                         .w = quaternionA.element.w + quaternionB.element.w,
                                         .x = quaternionA.element.x + quaternionB.element.x,
                                         .y = quaternionA.element.y + quaternionB.element.y,
                                         .z = quaternionA.element.z + quaternionB.element.z,
                                     }};
    return result;
 }
 /**
 * @brief Returns the multiplication of two quaternions.
 * @param quaternionA Quaternion A (to be post-multiplied).
 * @param quaternionB Quaternion B (to be pre-multiplied).
 * @return Multiplication of two quaternions.
 */
 static inline FusionQuaternion FusionQuaternionMultiply(const FusionQuaternion quaternionA, const FusionQuaternion quaternionB)
 {
 #define A quaternionA.element
 #define B quaternionB.element
    const FusionQuaternion result = {.element = {
                                         .w = A.w * B.w - A.x * B.x - A.y * B.y - A.z * B.z,
                                         .x = A.w * B.x + A.x * B.w + A.y * B.z - A.z * B.y,
                                         .y = A.w * B.y - A.x * B.z + A.y * B.w + A.z * B.x,
                                         .z = A.w * B.z + A.x * B.y - A.y * B.x + A.z * B.w,
                                     }};
    return result;
 #undef A
 #undef B
 }
 /**
 * @brief Returns the multiplication of a quaternion with a vector.  This is a
 * normal quaternion multiplication where the vector is treated a
 * quaternion with a W element value of zero.  The quaternion is post-
 * multiplied by the vector.
 * @param quaternion Quaternion.
 * @param vector Vector.
 * @return Multiplication of a quaternion with a vector.
 */
 static inline FusionQuaternion FusionQuaternionMultiplyVector(const FusionQuaternion quaternion, const FusionVector vector)
 {
 #define Q quaternion.element
 #define V vector.axis
    const FusionQuaternion result = {.element = {
                                         .w = -Q.x * V.x - Q.y * V.y - Q.z * V.z,
                                         .x = Q.w * V.x + Q.y * V.z - Q.z * V.y,
                                         .y = Q.w * V.y - Q.x * V.z + Q.z * V.x,
                                         .z = Q.w * V.z + Q.x * V.y - Q.y * V.x,
                                     }};
    return result;
 #undef Q
 #undef V
 }
 /**
 * @brief Returns the normalised quaternion.
 * @param quaternion Quaternion.
 * @return Normalised quaternion.
 */
 static inline FusionQuaternion FusionQuaternionNormalise(const FusionQuaternion quaternion)
 {
 #define Q quaternion.element
 #ifdef FUSION_USE_NORMAL_SQRT
    const float magnitudeReciprocal = 1.0f / sqrtf(Q.w * Q.w + Q.x * Q.x + Q.y * Q.y + Q.z * Q.z);
 #else
    const float magnitudeReciprocal = FusionFastInverseSqrt(Q.w * Q.w + Q.x * Q.x + Q.y * Q.y + Q.z * Q.z);
 #endif
    const FusionQuaternion result = {.element = {
                                         .w = Q.w * magnitudeReciprocal,
                                         .x = Q.x * magnitudeReciprocal,
                                         .y = Q.y * magnitudeReciprocal,
                                         .z = Q.z * magnitudeReciprocal,
                                     }};
    return result;
 #undef Q
 }
 //------------------------------------------------------------------------------
 // Inline functions - Matrix operations
 /**
 * @brief Returns the multiplication of a matrix with a vector.
 * @param matrix Matrix.
 * @param vector Vector.
 * @return Multiplication of a matrix with a vector.
 */
 static inline FusionVector FusionMatrixMultiplyVector(const FusionMatrix matrix, const FusionVector vector)
 {
 #define R matrix.element
    const FusionVector result = {.axis = {
                                     .x = R.xx * vector.axis.x + R.xy * vector.axis.y + R.xz * vector.axis.z,
                                     .y = R.yx * vector.axis.x + R.yy * vector.axis.y + R.yz * vector.axis.z,
                                     .z = R.zx * vector.axis.x + R.zy * vector.axis.y + R.zz * vector.axis.z,
                                 }};
    return result;
 #undef R
 }
 //------------------------------------------------------------------------------
 // Inline functions - Conversion operations
 /**
 * @brief Converts a quaternion to a rotation matrix.
 * @param quaternion Quaternion.
 * @return Rotation matrix.
 */
 static inline FusionMatrix FusionQuaternionToMatrix(const FusionQuaternion quaternion)
 {
 #define Q quaternion.element
    const float qwqw = Q.w * Q.w; // calculate common terms to avoid repeated operations
    const float qwqx = Q.w * Q.x;
    const float qwqy = Q.w * Q.y;
    const float qwqz = Q.w * Q.z;
    const float qxqy = Q.x * Q.y;
    const float qxqz = Q.x * Q.z;
    const float qyqz = Q.y * Q.z;
    const FusionMatrix matrix = {.element = {
                                     .xx = 2.0f * (qwqw - 0.5f + Q.x * Q.x),
                                     .xy = 2.0f * (qxqy - qwqz),
                                     .xz = 2.0f * (qxqz + qwqy),
                                     .yx = 2.0f * (qxqy + qwqz),
                                     .yy = 2.0f * (qwqw - 0.5f + Q.y * Q.y),
                                     .yz = 2.0f * (qyqz - qwqx),
                                     .zx = 2.0f * (qxqz - qwqy),
                                     .zy = 2.0f * (qyqz + qwqx),
                                     .zz = 2.0f * (qwqw - 0.5f + Q.z * Q.z),
                                 }};
    return matrix;
 #undef Q
 }
 /**
 * @brief Converts a quaternion to ZYX Euler angles in degrees.
 * @param quaternion Quaternion.
 * @return Euler angles in degrees.
 */
 static inline FusionEuler FusionQuaternionToEuler(const FusionQuaternion quaternion)
 {
 #define Q quaternion.element
    const float halfMinusQySquared = 0.5f - Q.y * Q.y; // calculate common terms to avoid repeated operations
    const FusionEuler euler = {.angle = {
                                   .roll = FusionRadiansToDegrees(atan2f(Q.w * Q.x + Q.y * Q.z, halfMinusQySquared - Q.x * Q.x)),
                                   .pitch = FusionRadiansToDegrees(FusionAsin(2.0f * (Q.w * Q.y - Q.z * Q.x))),
                                   .yaw = FusionRadiansToDegrees(atan2f(Q.w * Q.z + Q.x * Q.y, halfMinusQySquared - Q.z * Q.z)),
                               }};
    return euler;
 #undef Q
 }
 #endif
 //------------------------------------------------------------------------------
 // End of file
@@ -1,80 +0,0 @@
 /**
 * @file FusionOffset.c
 * @author Seb Madgwick
 * @brief Gyroscope offset correction algorithm for run-time calibration of the
 * gyroscope offset.
 */
 //------------------------------------------------------------------------------
 // Includes
 #include "FusionOffset.h"
 #include <math.h> // fabsf
 //------------------------------------------------------------------------------
 // Definitions
 /**
 * @brief Cutoff frequency in Hz.
 */
 #define CUTOFF_FREQUENCY (0.02f)
 /**
 * @brief Timeout in seconds.
 */
 #define TIMEOUT (5)
 /**
 * @brief Threshold in degrees per second.
 */
 #define THRESHOLD (3.0f)
 //------------------------------------------------------------------------------
 // Functions
 /**
 * @brief Initialises the gyroscope offset algorithm.
 * @param offset Gyroscope offset algorithm structure.
 * @param sampleRate Sample rate in Hz.
 */
 void FusionOffsetInitialise(FusionOffset *const offset, const unsigned int sampleRate)
 {
    offset->filterCoefficient = 2.0f * (float)M_PI * CUTOFF_FREQUENCY * (1.0f / (float)sampleRate);
    offset->timeout = TIMEOUT * sampleRate;
    offset->timer = 0;
    offset->gyroscopeOffset = FUSION_VECTOR_ZERO;
 }
 /**
 * @brief Updates the gyroscope offset algorithm and returns the corrected
 * gyroscope measurement.
 * @param offset Gyroscope offset algorithm structure.
 * @param gyroscope Gyroscope measurement in degrees per second.
 * @return Corrected gyroscope measurement in degrees per second.
 */
 FusionVector FusionOffsetUpdate(FusionOffset *const offset, FusionVector gyroscope)
 {
    // Subtract offset from gyroscope measurement
    gyroscope = FusionVectorSubtract(gyroscope, offset->gyroscopeOffset);
    // Reset timer if gyroscope not stationary
    if ((fabsf(gyroscope.axis.x) > THRESHOLD) || (fabsf(gyroscope.axis.y) > THRESHOLD) || (fabsf(gyroscope.axis.z) > THRESHOLD)) {
        offset->timer = 0;
        return gyroscope;
    }
    // Increment timer while gyroscope stationary
    if (offset->timer < offset->timeout) {
        offset->timer++;
        return gyroscope;
    }
    // Adjust offset if timer has elapsed
    offset->gyroscopeOffset =
        FusionVectorAdd(offset->gyroscopeOffset, FusionVectorMultiplyScalar(gyroscope, offset->filterCoefficient));
    return gyroscope;
 }
 //------------------------------------------------------------------------------
 // End of file
@@ -1,40 +0,0 @@
 /**
 * @file FusionOffset.h
 * @author Seb Madgwick
 * @brief Gyroscope offset correction algorithm for run-time calibration of the
 * gyroscope offset.
 */
 #ifndef FUSION_OFFSET_H
 #define FUSION_OFFSET_H
 //------------------------------------------------------------------------------
 // Includes
 #include "FusionMath.h"
 //------------------------------------------------------------------------------
 // Definitions
 /**
 * @brief Gyroscope offset algorithm structure.  Structure members are used
 * internally and must not be accessed by the application.
 */
 typedef struct {
    float filterCoefficient;
    unsigned int timeout;
    unsigned int timer;
    FusionVector gyroscopeOffset;
 } FusionOffset;
 //------------------------------------------------------------------------------
 // Function declarations
 void FusionOffsetInitialise(FusionOffset *const offset, const unsigned int sampleRate);
 FusionVector FusionOffsetUpdate(FusionOffset *const offset, FusionVector gyroscope);
 #endif
 //------------------------------------------------------------------------------
 // End of file
@@ -53,11 +53,11 @@ int32_t BMX160Sensor::runOnce()
    // If we're set to one of the inverted positions
    if (config.display.compass_orientation > meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270) {
-        ma = FusionAxesSwap(ma, FusionAxesAlignmentNXNYPZ);
+        ma = FusionRemap(ma, FusionRemapAlignmentNXNYPZ);
-        ga = FusionAxesSwap(ga, FusionAxesAlignmentNXNYPZ);
+        ga = FusionRemap(ga, FusionRemapAlignmentNXNYPZ);
    }
-    float heading = FusionCompassCalculateHeading(FusionConventionNed, ga, ma);
+    float heading = FusionCompass(ga, ma, FusionConventionNed);
    heading = applyCompassOrientation(heading);
    if (screen)
@@ -93,11 +93,11 @@ int32_t ICM20948Sensor::runOnce()
    // If we're set to one of the inverted positions
    if (config.display.compass_orientation > meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270) {
-        ma = FusionAxesSwap(ma, FusionAxesAlignmentNXNYPZ);
+        ma = FusionRemap(ma, FusionRemapAlignmentNXNYPZ);
-        ga = FusionAxesSwap(ga, FusionAxesAlignmentNXNYPZ);
+        ga = FusionRemap(ga, FusionRemapAlignmentNXNYPZ);
    }
-    float heading = FusionCompassCalculateHeading(FusionConventionNed, ga, ma);
+    float heading = FusionCompass(ga, ma, FusionConventionNed);
    heading = applyCompassOrientation(heading);
    if (screen)
@@ -103,10 +103,10 @@ int32_t MMC5983MASensor::runOnce()
        FusionVector ga = {.axis = {accelX, accelY, accelZ}};
        FusionVector ma = {.axis = {magX, magY, magZ}};
        if (config.display.compass_orientation > meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270) {
-            ma = FusionAxesSwap(ma, FusionAxesAlignmentNXNYPZ);
+            ma = FusionRemap(ma, FusionRemapAlignmentNXNYPZ);
-            ga = FusionAxesSwap(ga, FusionAxesAlignmentNXNYPZ);
+            ga = FusionRemap(ga, FusionRemapAlignmentNXNYPZ);
        }
-        heading = FusionCompassCalculateHeading(FusionConventionNed, ga, ma) + MMC5983MA_HEADING_OFFSET_DEG;
+        heading = FusionCompass(ga, ma, FusionConventionNed) + MMC5983MA_HEADING_OFFSET_DEG;
    } else {
        heading = atan2f(magY, magX) * RAD_TO_DEG + MMC5983MA_HEADING_OFFSET_DEG;
    }