Jay Balar
Sound update
Dependencies: 4DGL-uLCD-SE Physac-MBED PinDetect SDFileSystem mbed-rtos mbed
Revision 8:005b0a85be70, committed 20 months ago
- Comitter:
- jstephens78
- Date:
- Tue Nov 15 05:42:41 2022 +0000
- Parent:
- 7:d27f97ac2bea
- Commit message:
- Playing with Fusion AHRS
Changed in this revision
--- a/Fusion.lib Tue Nov 15 04:20:38 2022 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -https://github.com/xioTechnologies/Fusion/#bbbdd630e031d183db85e698b2dff573f683faef
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Fusion/CMakeLists.txt Tue Nov 15 05:42:41 2022 +0000
@@ -0,0 +1,7 @@
+file(GLOB_RECURSE files "*.c")
+
+add_library(Fusion ${files})
+
+if(UNIX AND NOT APPLE)
+ target_link_libraries(Fusion m) # link math library for Linux
+endif()
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Fusion/Fusion.h Tue Nov 15 05:42:41 2022 +0000
@@ -0,0 +1,31 @@
+/**
+ * @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 "FusionMath.h"
+#include "FusionOffset.h"
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+//------------------------------------------------------------------------------
+// End of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Fusion/FusionAhrs.c Tue Nov 15 05:42:41 2022 +0000
@@ -0,0 +1,354 @@
+/**
+ * @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 <float.h> // FLT_MAX
+#include "FusionAhrs.h"
+#include "FusionCompass.h"
+#include <math.h> // atan2f, cosf, 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)
+
+//------------------------------------------------------------------------------
+// Functions
+
+/**
+ * @brief Initialises the AHRS algorithm structure.
+ * @param ahrs AHRS algorithm structure.
+ */
+void FusionAhrsInitialise(FusionAhrs *const ahrs) {
+ const FusionAhrsSettings settings = {
+ .gain = 0.5f,
+ .accelerationRejection = 90.0f,
+ .magneticRejection = 90.0f,
+ .rejectionTimeout = 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->halfAccelerometerFeedback = FUSION_VECTOR_ZERO;
+ ahrs->halfMagnetometerFeedback = FUSION_VECTOR_ZERO;
+ ahrs->accelerometerIgnored = false;
+ ahrs->accelerationRejectionTimer = 0;
+ ahrs->accelerationRejectionTimeout = false;
+ ahrs->magnetometerIgnored = false;
+ ahrs->magneticRejectionTimer = 0;
+ ahrs->magneticRejectionTimeout = false;
+}
+
+/**
+ * @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.gain = settings->gain;
+ if ((settings->accelerationRejection == 0.0f) || (settings->rejectionTimeout == 0)) {
+ ahrs->settings.accelerationRejection = FLT_MAX;
+ } else {
+ ahrs->settings.accelerationRejection = powf(0.5f * sinf(FusionDegreesToRadians(settings->accelerationRejection)), 2);
+ }
+ if ((settings->magneticRejection == 0.0f) || (settings->rejectionTimeout == 0)) {
+ ahrs->settings.magneticRejection = FLT_MAX;
+ } else {
+ ahrs->settings.magneticRejection = powf(0.5f * sinf(FusionDegreesToRadians(settings->magneticRejection)), 2);
+ }
+ ahrs->settings.rejectionTimeout = settings->rejectionTimeout;
+ 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;
+
+ // Ramp down gain during initialisation
+ if (ahrs->initialising == true) {
+ ahrs->rampedGain -= ahrs->rampedGainStep * deltaTime;
+ if (ahrs->rampedGain < ahrs->settings.gain) {
+ ahrs->rampedGain = ahrs->settings.gain;
+ ahrs->initialising = false;
+ ahrs->accelerationRejectionTimeout = false;
+ }
+ }
+
+ // Calculate direction of gravity indicated by algorithm
+ const FusionVector halfGravity = {
+ .axis.x = Q.x * Q.z - Q.w * Q.y,
+ .axis.y = Q.y * Q.z + Q.w * Q.x,
+ .axis.z = Q.w * Q.w - 0.5f + Q.z * Q.z,
+ }; // third column of transposed rotation matrix scaled by 0.5
+
+ // Calculate accelerometer feedback
+ FusionVector halfAccelerometerFeedback = FUSION_VECTOR_ZERO;
+ ahrs->accelerometerIgnored = true;
+ if (FusionVectorIsZero(accelerometer) == false) {
+
+ // Enter acceleration recovery state if acceleration rejection times out
+ if (ahrs->accelerationRejectionTimer > ahrs->settings.rejectionTimeout) {
+ const FusionQuaternion quaternion = ahrs->quaternion;
+ FusionAhrsReset(ahrs);
+ ahrs->quaternion = quaternion;
+ ahrs->accelerationRejectionTimer = 0;
+ ahrs->accelerationRejectionTimeout = true;
+ }
+
+ // Calculate accelerometer feedback scaled by 0.5
+ ahrs->halfAccelerometerFeedback = FusionVectorCrossProduct(FusionVectorNormalise(accelerometer), halfGravity);
+
+ // Ignore accelerometer if acceleration distortion detected
+ if ((ahrs->initialising == true) || (FusionVectorMagnitudeSquared(ahrs->halfAccelerometerFeedback) <= ahrs->settings.accelerationRejection)) {
+ halfAccelerometerFeedback = ahrs->halfAccelerometerFeedback;
+ ahrs->accelerometerIgnored = false;
+ ahrs->accelerationRejectionTimer -= ahrs->accelerationRejectionTimer >= 10 ? 10 : 0;
+ } else {
+ ahrs->accelerationRejectionTimer++;
+ }
+ }
+
+ // Calculate magnetometer feedback
+ FusionVector halfMagnetometerFeedback = FUSION_VECTOR_ZERO;
+ ahrs->magnetometerIgnored = true;
+ if (FusionVectorIsZero(magnetometer) == false) {
+
+ // Set to compass heading if magnetic rejection times out
+ ahrs->magneticRejectionTimeout = false;
+ if (ahrs->magneticRejectionTimer > ahrs->settings.rejectionTimeout) {
+ FusionAhrsSetHeading(ahrs, FusionCompassCalculateHeading(halfGravity, magnetometer));
+ ahrs->magneticRejectionTimer = 0;
+ ahrs->magneticRejectionTimeout = true;
+ }
+
+ // Compute direction of west indicated by algorithm
+ const FusionVector halfWest = {
+ .axis.x = Q.x * Q.y + Q.w * Q.z,
+ .axis.y = Q.w * Q.w - 0.5f + Q.y * Q.y,
+ .axis.z = Q.y * Q.z - Q.w * Q.x
+ }; // second column of transposed rotation matrix scaled by 0.5
+
+ // Calculate magnetometer feedback scaled by 0.5
+ ahrs->halfMagnetometerFeedback = FusionVectorCrossProduct(FusionVectorNormalise(FusionVectorCrossProduct(halfGravity, magnetometer)), halfWest);
+
+ // Ignore magnetometer if magnetic distortion detected
+ if ((ahrs->initialising == true) || (FusionVectorMagnitudeSquared(ahrs->halfMagnetometerFeedback) <= ahrs->settings.magneticRejection)) {
+ halfMagnetometerFeedback = ahrs->halfMagnetometerFeedback;
+ ahrs->magnetometerIgnored = false;
+ ahrs->magneticRejectionTimer -= ahrs->magneticRejectionTimer >= 10 ? 10 : 0;
+ } else {
+ ahrs->magneticRejectionTimer++;
+ }
+ }
+
+ // 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 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 == true) && (ahrs->accelerationRejectionTimeout == false)) {
+ 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),
+ .axis.y = -1.0f * cosf(roll) * sinHeadingRadians,
+ .axis.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 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
+ const FusionVector gravity = {
+ .axis.x = 2.0f * (Q.x * Q.z - Q.w * Q.y),
+ .axis.y = 2.0f * (Q.y * Q.z + Q.w * Q.x),
+ .axis.z = 2.0f * (Q.w * Q.w - 0.5f + Q.z * Q.z),
+ }; // third column of transposed rotation matrix
+ const FusionVector linearAcceleration = FusionVectorSubtract(ahrs->accelerometer, gravity);
+ return linearAcceleration;
+#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
+ 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 FusionVector earthAcceleration = {
+ .axis.x = 2.0f * ((qwqw - 0.5f + Q.x * Q.x) * A.x + (qxqy - qwqz) * A.y + (qxqz + qwqy) * A.z),
+ .axis.y = 2.0f * ((qxqy + qwqz) * A.x + (qwqw - 0.5f + Q.y * Q.y) * A.y + (qyqz - qwqx) * A.z),
+ .axis.z = (2.0f * ((qxqz - qwqy) * A.x + (qyqz + qwqx) * A.y + (qwqw - 0.5f + Q.z * Q.z) * A.z)) - 1.0f,
+ }; // rotation matrix multiplied with the accelerometer, with 1 g subtracted
+ return earthAcceleration;
+#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,
+ .accelerationRejectionTimer = ahrs->settings.rejectionTimeout == 0 ? 0.0f : (float) ahrs->accelerationRejectionTimer / (float) ahrs->settings.rejectionTimeout,
+ .magneticError = FusionRadiansToDegrees(FusionAsin(2.0f * FusionVectorMagnitude(ahrs->halfMagnetometerFeedback))),
+ .magnetometerIgnored = ahrs->magnetometerIgnored,
+ .magneticRejectionTimer = ahrs->settings.rejectionTimeout == 0 ? 0.0f : (float) ahrs->magneticRejectionTimer / (float) ahrs->settings.rejectionTimeout,
+ };
+ return internalStates;
+}
+
+/**
+ * @brief Returns the AHRS algorithm flags.
+ * @param ahrs AHRS algorithm structure.
+ * @return AHRS algorithm flags.
+ */
+FusionAhrsFlags FusionAhrsGetFlags(FusionAhrs *const ahrs) {
+ const unsigned int warningTimeout = ahrs->settings.rejectionTimeout / 4;
+ const FusionAhrsFlags flags = {
+ .initialising = ahrs->initialising,
+ .accelerationRejectionWarning = ahrs->accelerationRejectionTimer > warningTimeout,
+ .accelerationRejectionTimeout = ahrs->accelerationRejectionTimeout,
+ .magneticRejectionWarning = ahrs->magneticRejectionTimer > warningTimeout,
+ .magneticRejectionTimeout = ahrs->magneticRejectionTimeout,
+ };
+ 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),
+ .element.x = 0.0f,
+ .element.y = 0.0f,
+ .element.z = -1.0f * sinf(halfYawMinusHeading),
+ };
+ ahrs->quaternion = FusionQuaternionMultiply(rotation, ahrs->quaternion);
+#undef Q
+}
+
+//------------------------------------------------------------------------------
+// End of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Fusion/FusionAhrs.h Tue Nov 15 05:42:41 2022 +0000
@@ -0,0 +1,104 @@
+/**
+ * @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 "FusionMath.h"
+#include <stdbool.h>
+
+//------------------------------------------------------------------------------
+// Definitions
+
+/**
+ * @brief AHRS algorithm settings.
+ */
+typedef struct {
+ float gain;
+ float accelerationRejection;
+ float magneticRejection;
+ unsigned int rejectionTimeout;
+} 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;
+ FusionVector halfAccelerometerFeedback;
+ FusionVector halfMagnetometerFeedback;
+ bool accelerometerIgnored;
+ unsigned int accelerationRejectionTimer;
+ bool accelerationRejectionTimeout;
+ bool magnetometerIgnored;
+ unsigned int magneticRejectionTimer;
+ bool magneticRejectionTimeout;
+} FusionAhrs;
+
+/**
+ * @brief AHRS algorithm internal states.
+ */
+typedef struct {
+ float accelerationError;
+ bool accelerometerIgnored;
+ float accelerationRejectionTimer;
+ float magneticError;
+ bool magnetometerIgnored;
+ float magneticRejectionTimer;
+} FusionAhrsInternalStates;
+
+/**
+ * @brief AHRS algorithm flags.
+ */
+typedef struct {
+ bool initialising;
+ bool accelerationRejectionWarning;
+ bool accelerationRejectionTimeout;
+ bool magneticRejectionWarning;
+ bool magneticRejectionTimeout;
+} 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);
+
+FusionVector FusionAhrsGetLinearAcceleration(const FusionAhrs *const ahrs);
+
+FusionVector FusionAhrsGetEarthAcceleration(const FusionAhrs *const ahrs);
+
+FusionAhrsInternalStates FusionAhrsGetInternalStates(const FusionAhrs *const ahrs);
+
+FusionAhrsFlags FusionAhrsGetFlags(FusionAhrs *const ahrs);
+
+void FusionAhrsSetHeading(FusionAhrs *const ahrs, const float heading);
+
+#endif
+
+//------------------------------------------------------------------------------
+// End of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Fusion/FusionAxes.h Tue Nov 15 05:42:41 2022 +0000
@@ -0,0 +1,187 @@
+/**
+ * @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;
+}
+
+#endif
+
+//------------------------------------------------------------------------------
+// End of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Fusion/FusionCalibration.h Tue Nov 15 05:42:41 2022 +0000
@@ -0,0 +1,44 @@
+/**
+ * @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 FusionVectorSubtract(FusionMatrixMultiplyVector(softIronMatrix, uncalibrated), hardIronOffset);
+}
+
+#endif
+
+//------------------------------------------------------------------------------
+// End of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Fusion/FusionCompass.c Tue Nov 15 05:42:41 2022 +0000
@@ -0,0 +1,36 @@
+/**
+ * @file FusionCompass.c
+ * @author Seb Madgwick
+ * @brief Tilt-compensated compass to calculate an heading relative to magnetic
+ * north using accelerometer and magnetometer measurements.
+ */
+
+//------------------------------------------------------------------------------
+// Includes
+
+#include "FusionCompass.h"
+#include <math.h> // atan2f
+
+//------------------------------------------------------------------------------
+// Functions
+
+/**
+ * @brief Calculates the heading relative to magnetic north.
+ * @param accelerometer Accelerometer measurement in any calibrated units.
+ * @param magnetometer Magnetometer measurement in any calibrated units.
+ * @return Heading angle in degrees.
+ */
+float FusionCompassCalculateHeading(const FusionVector accelerometer, const FusionVector magnetometer) {
+
+ // Compute direction of magnetic west (Earth's y axis)
+ const FusionVector magneticWest = FusionVectorNormalise(FusionVectorCrossProduct(accelerometer, magnetometer));
+
+ // Compute direction of magnetic north (Earth's x axis)
+ const FusionVector magneticNorth = FusionVectorNormalise(FusionVectorCrossProduct(magneticWest, accelerometer));
+
+ // Calculate angular heading relative to magnetic north
+ return FusionRadiansToDegrees(atan2f(magneticWest.axis.x, magneticNorth.axis.x));
+}
+
+//------------------------------------------------------------------------------
+// End of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Fusion/FusionCompass.h Tue Nov 15 05:42:41 2022 +0000 @@ -0,0 +1,24 @@ +/** + * @file FusionCompass.h + * @author Seb Madgwick + * @brief Tilt-compensated compass to calculate an heading relative to magnetic + * north using accelerometer and magnetometer measurements. + */ + +#ifndef FUSION_COMPASS_H +#define FUSION_COMPASS_H + +//------------------------------------------------------------------------------ +// Includes + +#include "FusionMath.h" + +//------------------------------------------------------------------------------ +// Function declarations + +float FusionCompassCalculateHeading(const FusionVector accelerometer, const FusionVector magnetometer); + +#endif + +//------------------------------------------------------------------------------ +// End of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Fusion/FusionMath.h Tue Nov 15 05:42:41 2022 +0000
@@ -0,0 +1,459 @@
+/**
+ * @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) {
+ FusionVector result;
+ result.axis.x = vectorA.axis.x + vectorB.axis.x;
+ result.axis.y = vectorA.axis.y + vectorB.axis.y;
+ result.axis.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) {
+ FusionVector result;
+ result.axis.x = vectorA.axis.x - vectorB.axis.x;
+ result.axis.y = vectorA.axis.y - vectorB.axis.y;
+ result.axis.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) {
+ FusionVector result;
+ result.axis.x = vector.axis.x * scalar;
+ result.axis.y = vector.axis.y * scalar;
+ result.axis.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) {
+ FusionVector result;
+ result.axis.x = vectorA.axis.x * vectorB.axis.x;
+ result.axis.y = vectorA.axis.y * vectorB.axis.y;
+ result.axis.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
+ FusionVector result;
+ result.axis.x = A.y * B.z - A.z * B.y;
+ result.axis.y = A.z * B.x - A.x * B.z;
+ result.axis.z = A.x * B.y - A.y * B.x;
+ return result;
+#undef A
+#undef B
+}
+
+/**
+ * @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) {
+ FusionQuaternion result;
+ result.element.w = quaternionA.element.w + quaternionB.element.w;
+ result.element.x = quaternionA.element.x + quaternionB.element.x;
+ result.element.y = quaternionA.element.y + quaternionB.element.y;
+ result.element.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
+ FusionQuaternion result;
+ result.element.w = A.w * B.w - A.x * B.x - A.y * B.y - A.z * B.z;
+ result.element.x = A.w * B.x + A.x * B.w + A.y * B.z - A.z * B.y;
+ result.element.y = A.w * B.y - A.x * B.z + A.y * B.w + A.z * B.x;
+ result.element.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
+ FusionQuaternion result;
+ result.element.w = -Q.x * V.x - Q.y * V.y - Q.z * V.z;
+ result.element.x = Q.w * V.x + Q.y * V.z - Q.z * V.y;
+ result.element.y = Q.w * V.y - Q.x * V.z + Q.z * V.x;
+ result.element.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
+ FusionQuaternion normalisedQuaternion;
+ normalisedQuaternion.element.w = Q.w * magnitudeReciprocal;
+ normalisedQuaternion.element.x = Q.x * magnitudeReciprocal;
+ normalisedQuaternion.element.y = Q.y * magnitudeReciprocal;
+ normalisedQuaternion.element.z = Q.z * magnitudeReciprocal;
+ return normalisedQuaternion;
+#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
+ FusionVector result;
+ result.axis.x = R.xx * vector.axis.x + R.xy * vector.axis.y + R.xz * vector.axis.z;
+ result.axis.y = R.yx * vector.axis.x + R.yy * vector.axis.y + R.yz * vector.axis.z;
+ result.axis.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;
+ FusionMatrix matrix;
+ matrix.element.xx = 2.0f * (qwqw - 0.5f + Q.x * Q.x);
+ matrix.element.xy = 2.0f * (qxqy - qwqz);
+ matrix.element.xz = 2.0f * (qxqz + qwqy);
+ matrix.element.yx = 2.0f * (qxqy + qwqz);
+ matrix.element.yy = 2.0f * (qwqw - 0.5f + Q.y * Q.y);
+ matrix.element.yz = 2.0f * (qyqz - qwqx);
+ matrix.element.zx = 2.0f * (qxqz - qwqy);
+ matrix.element.zy = 2.0f * (qyqz + qwqx);
+ matrix.element.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
+ FusionEuler euler;
+ euler.angle.roll = FusionRadiansToDegrees(atan2f(Q.w * Q.x + Q.y * Q.z, halfMinusQySquared - Q.x * Q.x));
+ euler.angle.pitch = FusionRadiansToDegrees(FusionAsin(2.0f * (Q.w * Q.y - Q.z * Q.x)));
+ euler.angle.yaw = FusionRadiansToDegrees(atan2f(Q.w * Q.z + Q.x * Q.y, halfMinusQySquared - Q.z * Q.z));
+ return euler;
+#undef Q
+}
+
+#endif
+
+//------------------------------------------------------------------------------
+// End of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Fusion/FusionOffset.c Tue Nov 15 05:42:41 2022 +0000
@@ -0,0 +1,77 @@
+/**
+ * @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> // fabs
+
+//------------------------------------------------------------------------------
+// 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 ((fabs(gyroscope.axis.x) > THRESHOLD) || (fabs(gyroscope.axis.y) > THRESHOLD) || (fabs(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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Fusion/FusionOffset.h Tue Nov 15 05:42:41 2022 +0000
@@ -0,0 +1,40 @@
+/**
+ * @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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Fusion/LICENSE.md Tue Nov 15 05:42:41 2022 +0000 @@ -0,0 +1,22 @@ + +The MIT License (MIT) + +Copyright (c) 2021 x-io Technologies + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE.
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Fusion/README.md Tue Nov 15 05:42:41 2022 +0000 @@ -0,0 +1,108 @@ +[](https://github.com/xioTechnologies/Fusion/tags/) +[](https://github.com/xioTechnologies/Fusion/actions/workflows/main.yml) +[](https://pypi.org/project/imufusion/) +[](https://pypi.org/project/imufusion/) +[](https://opensource.org/licenses/MIT) + +# Fusion + +Fusion is a sensor fusion library for Inertial Measurement Units (IMUs), optimised for embedded systems. Fusion is a C library but is also available as the Python package [imufusion](https://pypi.org/project/imufusion/). Two example Python scripts, [simple_example.py](https://github.com/xioTechnologies/Fusion/blob/main/Python/simple_example.py) and [advanced_example.py](https://github.com/xioTechnologies/Fusion/blob/main/Python/advanced_example.py) are provided with example sensor data to demonstrate use of the package. + +## AHRS algorithm + +The Attitude And Heading Reference System (AHRS) algorithm combines gyroscope, accelerometer, and magnetometer data into a single measurement of orientation relative to the Earth. The algorithm also supports systems that use only a gyroscope and accelerometer, and systems that use a gyroscope and accelerometer combined with an external source of heading measurement such as GPS. + +The algorithm is based on the revised AHRS algorithm presented in chapter 7 of [Madgwick's PhD thesis](https://ethos.bl.uk/OrderDetails.do?uin=uk.bl.ethos.681552). This is a different algorithm to the better-known initial AHRS algorithm presented in chapter 3, commonly referred to as the *Madgwick algorithm*. + +The algorithm calculates the orientation as the integration of the gyroscope summed with a feedback term. The feedback term is equal to the error in the current measurement of orientation as determined by the other sensors, multiplied by a gain. The algorithm therefore functions as a complementary filter that combines high-pass filtered gyroscope measurements with low-pass filtered measurements from other sensors with a corner frequency determined by the gain. A low gain will 'trust' the gyroscope more and so be more susceptible to drift. A high gain will increase the influence of other sensors and the errors that result from accelerations and magnetic distortions. A gain of zero will ignore the other sensors so that the measurement of orientation is determined by only the gyroscope. + +### Initialisation + +Initialisation occurs when the algorithm starts for the first time and after an acceleration rejection timeout. During initialisation, the acceleration and magnetic rejection features are disabled and the gain is ramped down from 10 to the final value over a 3 second period. This allows the measurement of orientation to rapidly converges from an arbitrary initial value to the value indicated by the sensors. + +### Acceleration rejection + +The acceleration rejection feature reduces the errors that result from the accelerations of linear and rotational motion. Acceleration rejection works by comparing the instantaneous measurement of inclination provided by the accelerometer with the current measurement of inclination of the algorithm output. If the angular difference between these two inclinations is greater than a threshold then the accelerometer will be ignored for this algorithm update. This is equivalent to a dynamic gain that deceases as accelerations increase. + +Prolonged accelerations from linear and rotational motion may result in the accelerometer being unusable as a measurement of inclination. This is detected by the algorithm as an acceleration rejection timeout. An acceleration rejection timeout occurs when the number of algorithm updates that have ignored the accelerometer exceeds ten times the number that have used the accelerometer within a defined period. If an acceleration rejection timeout occurs then the algorithm will reinitialise. + +### Magnetic rejection + +The magnetic rejection feature reduces the errors that result from temporary magnetic distortions. Magnetic rejection works using the same principle as acceleration rejection operating on the magnetometer instead of the accelerometer and by comparing the measurements of heading instead of inclination. A magnetic rejection timeout will not cause the algorithm to reinitialise. If a magnetic rejection timeout occurs then the heading of the algorithm output will be set to the instantaneous measurement of heading provided by the magnetometer. + +### Algorithm outputs + +The algorithm provides three outputs: quaternion, linear acceleration, and Earth acceleration. The quaternion describes the orientation of the sensor relative to the Earth using the North-West-Up (NWU) convention. This can be converted to a rotation matrix using the `FusionQuaternionToMatrix` function or to Euler angles using the `FusionQuaternionToEuler` function. The linear acceleration is the accelerometer measurement with the 1 g of gravity removed. The Earth acceleration is the accelerometer measurement in the Earth coordinate frame with the 1 g of gravity removed. + +### Algorithm settings + +The AHRS algorithm settings are defined by the `FusionAhrsSettings` structure and set using the `FusionAhrsSetSettings` function. + +| Setting | Description | +|-------------------------|---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| `gain` | Determines the influence of the gyroscope relative to other sensors. A value of 0.5 is appropriate for most applications. | +| `accelerationRejection` | Threshold (in degrees) used by the acceleration rejection feature. A value of zero will disable this feature. A value of 10 degrees is appropriate for most applications. | +| `magneticRejection` | Threshold (in degrees) used by the magnetic rejection feature. A value of zero will disable the feature. A value of 20 degrees is appropriate for most applications. | +| `rejectionTimeout` | Acceleration and magnetic rejection timeout period (in samples). A value of zero will disable the acceleration and magnetic rejection features. A period of 5 seconds is appropriate for most applications. | + +### Algorithm internal states + +The AHRS algorithm internal states are defined by the `FusionAhrsInternalStates` structure and obtained using the `FusionAhrsGetInternalStates` function. + +| State | Description | +|------------------------------|------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| `accelerationError` | Angular error (in degrees) of the instantaneous measurement of inclination provided by the accelerometer. The acceleration rejection feature will ignore the accelerometer if this value exceeds the `accelerationRejection` threshold set in the algorithm settings. | +| `accelerometerIgnored` | `true` if the accelerometer was ignored by the previous algorithm update. | +| `accelerationRejectionTimer` | Acceleration rejection timer value normalised to between 0.0 and 1.0. An acceleration rejection timeout will occur when this value reaches 1.0. | +| `magneticError` | Angular error (in degrees) of the instantaneous measurement of heading provided by the magnetometer. The magnetic rejection feature will ignore the magnetometer if this value exceeds the `magneticRejection` threshold set in the algorithm settings. | +| `magnetometerIgnored` | `true` if the magnetometer was ignored by the previous algorithm update. | +| `magneticRejectionTimer` | Magnetic rejection timer value normalised to between 0.0 and 1.0. A magnetic rejection timeout will occur when this value reaches 1.0. | + +### Algorithm flags + +The AHRS algorithm flags are defined by the `FusionAhrsFlags` structure and obtained using the `FusionAhrsGetFlags` function. + +| Flag | Description | +|--------------------------------|----------------------------------------------------------------------------------------------------------------------------| +| `initialising` | `true` if the algorithm is initialising. | +| `accelerationRejectionWarning` | `true` if the acceleration rejection timer has exceeded 25% of the `rejectionTimeout` value set in the algorithm settings. | +| `accelerationRejectionTimeout` | `true` if an acceleration rejection timeout has occurred and the algorithm is initialising. | +| `magneticRejectionWarning` | `true` if the magnetic rejection timer has exceeded 25% of the `rejectionTimeout` value set in the algorithm settings. | +| `magneticRejectionTimeout` | `true` if a magnetic rejection timeout has occurred during the previous algorithm update. | + +## Gyroscope offset correction algorithm + +The gyroscope offset correction algorithm provides run-time calibration of the gyroscope offset to compensate for variations in temperature and fine-tune existing offset calibration that may already be in place. This algorithm should be used in conjunction with the AHRS algorithm to achieve best performance. + +The algorithm calculates the gyroscope offset by detecting the stationary periods that occur naturally in most applications. Gyroscope measurements are sampled during these periods and low-pass filtered to obtain the gyroscope offset. The algorithm requires that gyroscope measurements do not exceed +/-3 degrees per second while stationary. Basic gyroscope offset calibration may be necessary to ensure that the initial offset error plus measurement noise is within these bounds. + +## Sensor calibration + +Sensor calibration is essential for accurate measurements. This library provides functions to apply calibration parameters to the gyroscope, accelerometer, and magnetometer. This library does not provide a solution for calculating the calibration parameters. + +### Inertial calibration + +The `FusionCalibrationInertial` function applies gyroscope and accelerometer calibration parameters using the calibration model: + +i<sub>c</sub> = Ms(i<sub>u</sub> - b) + +- i<sub>c</sub> is the calibrated inertial measurement and `return` value +- i<sub>u</sub> is the uncalibrated inertial measurement and `uncalibrated` argument +- M is the misalignment matrix and `misalignment` argument +- s is the sensitivity diagonal matrix and `sensitivity` argument +- b is the offset vector and `offset` argument + +### Magnetic calibration + +The `FusionCalibrationMagnetic` function applies magnetometer calibration parameters using the calibration model: + +m<sub>c</sub> = Sm<sub>u</sub> - h + +- m<sub>c</sub> is the calibrated magnetometer measurement and `return` value +- m<sub>u</sub> is the uncalibrated magnetometer measurement and `uncalibrated` argument +- S is the soft iron matrix and `softIronMatrix` argument +- h is the hard iron offset vector and `hardIronOffset` argument + +## Fast inverse square root + +Fusion uses [Pizer's implementation](https://pizer.wordpress.com/2008/10/12/fast-inverse-square-root/) of the [fast inverse square root](https://en.wikipedia.org/wiki/Fast_inverse_square_root) algorithm for vector and quaternion normalisation. Including the definition `FUSION_USE_NORMAL_SQRT` in [FusionMath.h](https://github.com/xioTechnologies/Fusion/blob/main/Fusion/FusionMath.h) or adding this as a preprocessor definition will use normal square root operations for all normalisation calculations. This will slow down execution speed for a small increase in accuracy. The increase in accuracy will typically be too small to observe on any practical system.
--- a/main.cpp Tue Nov 15 04:20:38 2022 +0000
+++ b/main.cpp Tue Nov 15 05:42:41 2022 +0000
@@ -1,10 +1,16 @@
-// uLCD-144-G2 demo program for uLCD-4GL LCD driver library
-//
+/**
+ * Uses the Fusion library for AHRS algorithm:
+ * https://github.com/xioTechnologies/Fusion
+ */
+
+
#include <cmath>
#include "mbed.h"
#include "uLCD_4DGL.h"
#include "LSM9DS1.h"
#define PI 3.14159
+#include "Fusion/Fusion.h"
+#define SAMPLE_RATE (100) // replace this with actual sample rate
uLCD_4DGL uLCD(p9,p10,p30);
LSM9DS1 imu(p28, p27, 0xD6, 0x3C);
@@ -23,10 +29,10 @@
if (az == 0.0) roll = (ay < 0.0) ? 180 : 0.0;
else roll = atan2(ay, az);
-
+
if (my == 0.0) yaw = (mx < 0.0) ? 180.0 : 0.0;
else yaw = atan2(mx, my);
-
+
if (yaw > 180.0) yaw -=PI;
else if (yaw < -180.0) yaw += PI;
@@ -34,35 +40,95 @@
int main()
{
+ ////////////////////
+ // INITIALIZATION //
+ ////////////////////
+
imu.begin();
if (!imu.begin()) {
pc.printf("Failed to communicate with LSM9DS1.\n");
}
+ imu.setGyroScale(2000);
+ imu.setAccelScale(16);
imu.calibrate(1);
+ // Initialise algorithms
+ FusionAhrs ahrs;
+ FusionAhrsInitialise(&ahrs);
+
+ // Set AHRS algorithm settings
+ const FusionAhrsSettings settings = {
+ .gain = 0.5f,
+ .accelerationRejection = 10.0f,
+ .magneticRejection = 20.0f,
+ .rejectionTimeout = 5 * SAMPLE_RATE, /* 5 seconds */
+ };
+ FusionAhrsSetSettings(&ahrs, &settings);
+
+ Timer timer;
+ timer.start();
+
+
+
+ //////////////
+ // IMU LOOP //
+ //////////////
while (true) {
// Read IMU
while(!imu.accelAvailable());
imu.readAccel();
+ while (!imu.gyroAvailable());
+ imu.readGyro();
+
while (!imu.magAvailable());
imu.readMag();
-
+ // Elementary Algorithm:
+ //float roll, pitch, yaw;
+ //getAttitude(imu, roll, pitch, yaw);
+ //pc.printf("%7f %7f %7f\r\n", roll, pitch, yaw);
- float roll, pitch, yaw;
- getAttitude(imu, roll, pitch, yaw);
+ // Construct algorithm inputs
+ FusionVector gyroscope = { // Gyro data in degrees / s
+ imu.calcGyro(imu.gx),
+ imu.calcGyro(imu.gy),
+ imu.calcGyro(imu.gz)
+ };
+ FusionVector accelerometer = { // Accelerometer data in g
+ imu.calcAccel(imu.ax),
+ imu.calcAccel(imu.ay),
+ imu.calcAccel(imu.az)
+ };
+ FusionVector magnetometer = { // Accelerometer data in g
+ imu.calcMag(imu.mx),
+ imu.calcMag(imu.my),
+ imu.calcMag(imu.mz)
+ };
- pc.printf("%7f %7f %7f\r\n", roll, pitch, yaw);
+ // Update AHRS algorithm
+ float dt = timer.read();
+ timer.reset();
+ FusionAhrsUpdate(&ahrs, gyroscope, accelerometer, magnetometer, dt);
+ // Print algorithm outputs
+ const FusionEuler euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs));
+ const FusionVector earth = FusionAhrsGetEarthAcceleration(&ahrs);
+
+ printf("R %5.1f P %5.1f Y %5.1f\n\r",
+ euler.angle.roll, euler.angle.pitch, euler.angle.yaw);
+
+ float roll = euler.angle.roll / 360 * 2 * PI,
+ pitch = euler.angle.pitch / 360 * 2 * PI,
+ yaw = euler.angle.yaw / 360 * 2 * PI;
uLCD.cls();
float x = 64 + roll * 60;
float y = 64 + pitch * 60;
- float dx = 12 * sin(yaw);
- float dy = 12 * cos(yaw);
+ float dx = 12 * cos(yaw);
+ float dy = 12 * sin(yaw);
uLCD.circle(x, y, 2, GREEN);
uLCD.line(x-dx, y-dy, x+dx, y+dy, GREEN);