From babbac3308ab4a8c71382433243ad04154c4e2cd Mon Sep 17 00:00:00 2001 From: Jonathan Bennett Date: Fri, 3 Jul 2026 15:59:18 -0500 Subject: [PATCH] Use upstream fusion library (#10724) * Use upstream fusion library * Update Fusion dependency to new repository and version --- platformio.ini | 2 + src/Fusion/Fusion.h | 32 -- src/Fusion/FusionAhrs.c | 542 --------------------------------- src/Fusion/FusionAhrs.h | 112 ------- src/Fusion/FusionAxes.h | 188 ------------ src/Fusion/FusionCalibration.h | 49 --- src/Fusion/FusionCompass.c | 51 ---- src/Fusion/FusionCompass.h | 26 -- src/Fusion/FusionConvention.h | 25 -- src/Fusion/FusionMath.h | 503 ------------------------------ src/Fusion/FusionOffset.c | 80 ----- src/Fusion/FusionOffset.h | 40 --- src/motion/BMX160Sensor.cpp | 6 +- src/motion/ICM20948Sensor.cpp | 6 +- src/motion/MMC5983MASensor.cpp | 6 +- 15 files changed, 11 insertions(+), 1657 deletions(-) delete mode 100644 src/Fusion/Fusion.h delete mode 100644 src/Fusion/FusionAhrs.c delete mode 100644 src/Fusion/FusionAhrs.h delete mode 100644 src/Fusion/FusionAxes.h delete mode 100644 src/Fusion/FusionCalibration.h delete mode 100644 src/Fusion/FusionCompass.c delete mode 100644 src/Fusion/FusionCompass.h delete mode 100644 src/Fusion/FusionConvention.h delete mode 100644 src/Fusion/FusionMath.h delete mode 100644 src/Fusion/FusionOffset.c delete mode 100644 src/Fusion/FusionOffset.h diff --git a/platformio.ini b/platformio.ini index 32a5d9d3e..8e8487f32 100644 --- a/platformio.ini +++ b/platformio.ini @@ -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 diff --git a/src/Fusion/Fusion.h b/src/Fusion/Fusion.h deleted file mode 100644 index 48f5198c5..000000000 --- a/src/Fusion/Fusion.h +++ /dev/null @@ -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 diff --git a/src/Fusion/FusionAhrs.c b/src/Fusion/FusionAhrs.c deleted file mode 100644 index d6c1d0215..000000000 --- a/src/Fusion/FusionAhrs.c +++ /dev/null @@ -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 // FLT_MAX -#include // 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 diff --git a/src/Fusion/FusionAhrs.h b/src/Fusion/FusionAhrs.h deleted file mode 100644 index aa2326e43..000000000 --- a/src/Fusion/FusionAhrs.h +++ /dev/null @@ -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 - -//------------------------------------------------------------------------------ -// 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 diff --git a/src/Fusion/FusionAxes.h b/src/Fusion/FusionAxes.h deleted file mode 100644 index 9673c88ff..000000000 --- a/src/Fusion/FusionAxes.h +++ /dev/null @@ -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 diff --git a/src/Fusion/FusionCalibration.h b/src/Fusion/FusionCalibration.h deleted file mode 100644 index be7102b73..000000000 --- a/src/Fusion/FusionCalibration.h +++ /dev/null @@ -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 diff --git a/src/Fusion/FusionCompass.c b/src/Fusion/FusionCompass.c deleted file mode 100644 index 6a6f9591a..000000000 --- a/src/Fusion/FusionCompass.c +++ /dev/null @@ -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 // 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 diff --git a/src/Fusion/FusionCompass.h b/src/Fusion/FusionCompass.h deleted file mode 100644 index a3d0b466a..000000000 --- a/src/Fusion/FusionCompass.h +++ /dev/null @@ -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 diff --git a/src/Fusion/FusionConvention.h b/src/Fusion/FusionConvention.h deleted file mode 100644 index 0b0d43adc..000000000 --- a/src/Fusion/FusionConvention.h +++ /dev/null @@ -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 diff --git a/src/Fusion/FusionMath.h b/src/Fusion/FusionMath.h deleted file mode 100644 index c3fc34b2d..000000000 --- a/src/Fusion/FusionMath.h +++ /dev/null @@ -1,503 +0,0 @@ -/** - * @file FusionMath.h - * @author Seb Madgwick - * @brief Math library. - */ - -#ifndef FUSION_MATH_H -#define FUSION_MATH_H - -//------------------------------------------------------------------------------ -// Includes - -#include // M_PI, sqrtf, atan2f, asinf -#include -#include - -//------------------------------------------------------------------------------ -// 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 diff --git a/src/Fusion/FusionOffset.c b/src/Fusion/FusionOffset.c deleted file mode 100644 index d4334c874..000000000 --- a/src/Fusion/FusionOffset.c +++ /dev/null @@ -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 // 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 diff --git a/src/Fusion/FusionOffset.h b/src/Fusion/FusionOffset.h deleted file mode 100644 index 51ae4a896..000000000 --- a/src/Fusion/FusionOffset.h +++ /dev/null @@ -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 diff --git a/src/motion/BMX160Sensor.cpp b/src/motion/BMX160Sensor.cpp index 02303faa4..863a23f57 100755 --- a/src/motion/BMX160Sensor.cpp +++ b/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) diff --git a/src/motion/ICM20948Sensor.cpp b/src/motion/ICM20948Sensor.cpp index e44994a60..3dd5a5e46 100755 --- a/src/motion/ICM20948Sensor.cpp +++ b/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) diff --git a/src/motion/MMC5983MASensor.cpp b/src/motion/MMC5983MASensor.cpp index 3789c6136..d63420d92 100644 --- a/src/motion/MMC5983MASensor.cpp +++ b/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; }