Use upstream fusion library

* add ScanI2C for IIS2MDCTR and ISM330DHCX

* Trunk format

* Minor cleanup

Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com>

---------

Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com>

platformio.ini

@@ -200,6 +200,7 @@ 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
+	https://github.com/xioTechnologies/Fusion/archive/a93c0dc83ce3ab65246f63ba134d3c2a15d6cabf.zip
 
 ; Common environmental sensor libraries (not included in native / portduino)
 [environmental_extra_common]

src/Fusion/Fusion.h

@@ -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

src/Fusion/FusionAhrs.c

@@ -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

src/Fusion/FusionAhrs.h

@@ -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

src/Fusion/FusionAxes.h

@@ -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

src/Fusion/FusionCalibration.h

@@ -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

src/Fusion/FusionCompass.c

@@ -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

src/Fusion/FusionCompass.h

@@ -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

src/Fusion/FusionConvention.h

@@ -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

src/Fusion/FusionMath.h

@@ -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

src/Fusion/FusionOffset.c

@@ -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

src/Fusion/FusionOffset.h

@@ -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

src/detect/ScanI2C.cpp

@@ -37,15 +37,15 @@ ScanI2C::FoundDevice ScanI2C::firstKeyboard() const
 
 ScanI2C::FoundDevice ScanI2C::firstAccelerometer() const
 {
-    ScanI2C::DeviceType types[] = {MPU6050,  LIS3DH,   BMA423, LSM6DS3, BMX160,   STK8BAXX,
-                                   ICM20948, QMA6100P, BMM150, BMI270,  ICM42607P};
-    return firstOfOrNONE(11, types);
+    ScanI2C::DeviceType types[] = {MPU6050,  LIS3DH,   BMA423, LSM6DS3, BMX160,    STK8BAXX,
+                                   ICM20948, QMA6100P, BMM150, BMI270,  ICM42607P, ISM330DHCX};
+    return firstOfOrNONE(12, types);
 }
 
 ScanI2C::FoundDevice ScanI2C::firstMagnetometer() const
 {
-    ScanI2C::DeviceType types[] = {MMC5983MA};
-    return firstOfOrNONE(1, types);
+    ScanI2C::DeviceType types[] = {MMC5983MA, IIS2MDCTR};
+    return firstOfOrNONE(2, types);
 }
 
 ScanI2C::FoundDevice ScanI2C::firstAQI() const

src/detect/ScanI2C.h

@@ -99,6 +99,8 @@ class ScanI2C
         CW2015,
         SCD30,
         ADS1115,
+        IIS2MDCTR,
+        ISM330DHCX,
     } DeviceType;
 
     // typedef uint8_t DeviceAddress;

src/detect/ScanI2CTwoWire.cpp

@@ -584,6 +584,9 @@ void ScanI2CTwoWire::scanPort(I2CPort port, uint8_t *address, uint8_t asize)
                 if (registerValue == 0x6A) {
                     type = LSM6DS3;
                     logFoundDevice("LSM6DS3", (uint8_t)addr.address);
+                } else if (registerValue == 0x6B) {
+                    type = ISM330DHCX;
+                    logFoundDevice("ISM330DHCX", (uint8_t)addr.address);
                 } else {
                     type = QMI8658;
                     logFoundDevice("QMI8658", (uint8_t)addr.address);
@@ -591,7 +594,17 @@ void ScanI2CTwoWire::scanPort(I2CPort port, uint8_t *address, uint8_t asize)
                 break;
 
                 SCAN_SIMPLE_CASE(QMC5883L_ADDR, QMC5883L, "QMC5883L", (uint8_t)addr.address)
-                SCAN_SIMPLE_CASE(HMC5883L_ADDR, HMC5883L, "HMC5883L", (uint8_t)addr.address)
+            case HMC5883L_ADDR:
+                registerValue = getRegisterValue(ScanI2CTwoWire::RegisterLocation(addr, 0x4FU), 1); // get ID
+                if (registerValue == 0x40) {
+                    type = IIS2MDCTR;
+                    logFoundDevice("IIS2MDCTR", (uint8_t)addr.address);
+                    break;
+                } else {
+                    type = HMC5883L;
+                    logFoundDevice("HMC5883L", (uint8_t)addr.address);
+                    break;
+                }
 #ifdef HAS_QMA6100P
                 SCAN_SIMPLE_CASE(QMA6100P_ADDR, QMA6100P, "QMA6100P", (uint8_t)addr.address)
 #else

src/motion/BMX160Sensor.cpp

@@ -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);
-        ga = FusionAxesSwap(ga, FusionAxesAlignmentNXNYPZ);
+        ma = FusionRemap(ma, FusionRemapAlignmentNXNYPZ);
+        ga = FusionRemap(ga, FusionRemapAlignmentNXNYPZ);
     }
 
-    float heading = FusionCompassCalculateHeading(FusionConventionNed, ga, ma);
+    float heading = FusionCompass(ga, ma, FusionConventionNed);
 
     heading = applyCompassOrientation(heading);
     if (screen)

src/motion/ICM20948Sensor.cpp

@@ -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);
-        ga = FusionAxesSwap(ga, FusionAxesAlignmentNXNYPZ);
+        ma = FusionRemap(ma, FusionRemapAlignmentNXNYPZ);
+        ga = FusionRemap(ga, FusionRemapAlignmentNXNYPZ);
     }
 
-    float heading = FusionCompassCalculateHeading(FusionConventionNed, ga, ma);
+    float heading = FusionCompass(ga, ma, FusionConventionNed);
 
     heading = applyCompassOrientation(heading);
     if (screen)

src/motion/MMC5983MASensor.cpp

@@ -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);
-            ga = FusionAxesSwap(ga, FusionAxesAlignmentNXNYPZ);
+            ma = FusionRemap(ma, FusionRemapAlignmentNXNYPZ);
+            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;
     }

variants/nrf52840/heltec_mesh_node_t1/variant.cpp

@@ -25,8 +25,7 @@
 
 const uint32_t g_ADigitalPinMap[] = {
     // P0 - pins 0 and 1 are hardwired for xtal and should never be enabled
-    0xff, 0xff, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25,
-    26,   27,   28, 29, 30, 31,
+    0xff, 0xff, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31,
 
     // P1
     32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47};