rbxii3/firmware
1
0
Fork 0
mirror of https://github.com/meshtastic/firmware.git synced 2026-01-23 18:27:27 +00:00
Code Issues Packages Projects Releases Wiki Activity

add a .clang-format file (#9154)

Browse Source
This commit is contained in:
Jorropo
2026-01-03 21:19:24 +01:00
committed by GitHub
parent abab6ce815
commit 0d11331d18
771 changed files with 77752 additions and 83184 deletions
Download Patch File Download Diff File Expand all files Collapse all files

622
src/Fusion/FusionAhrs.c
View File

@@ -43,18 +43,17 @@ static inline int Clamp(const int value, const int min, const int max);
* @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);
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);
}
/**
@@ -62,21 +61,20 @@ void FusionAhrsInitialise(FusionAhrs *const ahrs)
* 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;
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;
}
/**
@@ -84,28 +82,25 @@ void FusionAhrsReset(FusionAhrs *const ahrs)
* @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;
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;
}
/**
@@ -117,119 +112,113 @@ void FusionAhrsSetSettings(FusionAhrs *const ahrs, const FusionAhrsSettings *con
* @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)
{
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;
// 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;
// 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;
}
// 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;
}
// 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;
}
// 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;
}
// 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);
// 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;
}
// 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));
// 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));
// 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)));
// Integrate rate of change of quaternion
ahrs->quaternion = FusionQuaternionAdd(
ahrs->quaternion, FusionQuaternionMultiplyVector(ahrs->quaternion, FusionVectorMultiplyScalar(adjustedHalfGyroscope, deltaTime)));
// Normalise quaternion
ahrs->quaternion = FusionQuaternionNormalise(ahrs->quaternion);
// Normalise quaternion
ahrs->quaternion = FusionQuaternionNormalise(ahrs->quaternion);
#undef Q
}
@@ -238,29 +227,28 @@ void FusionAhrsUpdate(FusionAhrs *const ahrs, const FusionVector gyroscope, cons
* @param ahrs AHRS algorithm structure.
* @return Direction of gravity scaled by 0.5.
*/
static inline FusionVector HalfGravity(const FusionAhrs *const ahrs)
{
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
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
}
@@ -269,36 +257,35 @@ static inline FusionVector HalfGravity(const FusionAhrs *const ahrs)
* @param ahrs AHRS algorithm structure.
* @return Direction of the magnetic field scaled by 0.5.
*/
static inline FusionVector HalfMagnetic(const FusionAhrs *const ahrs)
{
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
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
}
@@ -308,12 +295,11 @@ static inline FusionVector HalfMagnetic(const FusionAhrs *const ahrs)
* @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);
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);
}
/**
@@ -323,15 +309,14 @@ static inline FusionVector Feedback(const FusionVector sensor, const FusionVecto
* @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;
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;
}
/**
@@ -342,17 +327,15 @@ static inline int Clamp(const int value, const int min, const int max)
* @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)
{
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);
// Update AHRS algorithm
FusionAhrsUpdate(ahrs, gyroscope, accelerometer, FUSION_VECTOR_ZERO, deltaTime);
// Zero heading during initialisation
if (ahrs->initialising) {
FusionAhrsSetHeading(ahrs, 0.0f);
}
// Zero heading during initialisation
if (ahrs->initialising) {
FusionAhrsSetHeading(ahrs, 0.0f);
}
}
/**
@@ -364,25 +347,24 @@ void FusionAhrsUpdateNoMagnetometer(FusionAhrs *const ahrs, const FusionVector 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)
{
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 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),
}};
// 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);
// Update AHRS algorithm
FusionAhrsUpdate(ahrs, gyroscope, accelerometer, magnetometer, deltaTime);
#undef Q
}
@@ -391,20 +373,14 @@ void FusionAhrsUpdateExternalHeading(FusionAhrs *const ahrs, const FusionVector
* @param ahrs AHRS algorithm structure.
* @return Quaternion describing the sensor relative to the Earth.
*/
FusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs *const ahrs)
{
return ahrs->quaternion;
}
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;
}
void FusionAhrsSetQuaternion(FusionAhrs *const ahrs, const FusionQuaternion quaternion) { ahrs->quaternion = quaternion; }
/**
* @brief Returns the linear acceleration measurement equal to the accelerometer
@@ -412,28 +388,27 @@ void FusionAhrsSetQuaternion(FusionAhrs *const ahrs, const FusionQuaternion quat
* @param ahrs AHRS algorithm structure.
* @return Linear acceleration measurement in g.
*/
FusionVector FusionAhrsGetLinearAcceleration(const FusionAhrs *const ahrs)
{
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
// 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
// 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
}
@@ -443,36 +418,35 @@ FusionVector FusionAhrsGetLinearAcceleration(const FusionAhrs *const ahrs)
* @param ahrs AHRS algorithm structure.
* @return Earth acceleration measurement in g.
*/
FusionVector FusionAhrsGetEarthAcceleration(const FusionAhrs *const ahrs)
{
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
// 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;
// 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
}
@@ -482,22 +456,18 @@ FusionVector FusionAhrsGetEarthAcceleration(const FusionAhrs *const ahrs)
* @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;
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;
}
/**
@@ -505,15 +475,14 @@ FusionAhrsInternalStates FusionAhrsGetInternalStates(const FusionAhrs *const ahr
* @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;
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;
}
/**
@@ -523,18 +492,17 @@ FusionAhrsFlags FusionAhrsGetFlags(const FusionAhrs *const ahrs)
* @param ahrs AHRS algorithm structure.
* @param heading Heading angle in degrees.
*/
void FusionAhrsSetHeading(FusionAhrs *const ahrs, const float heading)
{
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);
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
}

73
src/Fusion/FusionAhrs.h
View File

@@ -22,12 +22,12 @@
* @brief AHRS algorithm settings.
*/
typedef struct {
FusionConvention convention;
float gain;
float gyroscopeRange;
float accelerationRejection;
float magneticRejection;
unsigned int recoveryTriggerPeriod;
FusionConvention convention;
float gain;
float gyroscopeRange;
float accelerationRejection;
float magneticRejection;
unsigned int recoveryTriggerPeriod;
} FusionAhrsSettings;
/**
@@ -35,43 +35,43 @@ typedef struct {
* 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;
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;
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;
bool initialising;
bool angularRateRecovery;
bool accelerationRecovery;
bool magneticRecovery;
} FusionAhrsFlags;
//------------------------------------------------------------------------------
@@ -83,14 +83,13 @@ 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 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 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);
void FusionAhrsUpdateExternalHeading(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const float heading,
const float deltaTime);
FusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs *const ahrs);

293
src/Fusion/FusionAxes.h
View File

@@ -22,30 +22,30 @@
* 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 */
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;
//------------------------------------------------------------------------------
@@ -57,129 +57,128 @@ typedef enum {
* @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
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

13
src/Fusion/FusionCalibration.h
View File

@@ -23,11 +23,9 @@
* @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));
static inline FusionVector FusionCalibrationInertial(const FusionVector uncalibrated, const FusionMatrix misalignment, const FusionVector sensitivity,
const FusionVector offset) {
return FusionMatrixMultiplyVector(misalignment, FusionVectorHadamardProduct(FusionVectorSubtract(uncalibrated, offset), sensitivity));
}
/**
@@ -38,9 +36,8 @@ static inline FusionVector FusionCalibrationInertial(const FusionVector uncalibr
* @return Calibrated measurement.
*/
static inline FusionVector FusionCalibrationMagnetic(const FusionVector uncalibrated, const FusionMatrix softIronMatrix,
const FusionVector hardIronOffset)
{
return FusionMatrixMultiplyVector(softIronMatrix, FusionVectorSubtract(uncalibrated, hardIronOffset));
const FusionVector hardIronOffset) {
return FusionMatrixMultiplyVector(softIronMatrix, FusionVectorSubtract(uncalibrated, hardIronOffset));
}
#endif

44
src/Fusion/FusionCompass.c
View File

@@ -22,29 +22,27 @@
* @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
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
}
//------------------------------------------------------------------------------

3
src/Fusion/FusionCompass.h
View File

@@ -17,8 +17,7 @@
//------------------------------------------------------------------------------
// Function declarations
float FusionCompassCalculateHeading(const FusionConvention convention, const FusionVector accelerometer,
const FusionVector magnetometer);
float FusionCompassCalculateHeading(const FusionConvention convention, const FusionVector accelerometer, const FusionVector magnetometer);
#endif

6
src/Fusion/FusionConvention.h
View File

@@ -14,9 +14,9 @@
* @brief Earth axes convention.
*/
typedef enum {
FusionConventionNwu, /* North-West-Up */
FusionConventionEnu, /* East-North-Up */
FusionConventionNed, /* North-East-Down */
FusionConventionNwu, /* North-West-Up */
FusionConventionEnu, /* East-North-Up */
FusionConventionNed, /* North-East-Down */
} FusionConvention;
#endif

360
src/Fusion/FusionMath.h
View File

@@ -21,27 +21,27 @@
* @brief 3D vector.
*/
typedef union {
float array[3];
float array[3];
struct {
float x;
float y;
float z;
} axis;
struct {
float x;
float y;
float z;
} axis;
} FusionVector;
/**
* @brief Quaternion.
*/
typedef union {
float array[4];
float array[4];
struct {
float w;
float x;
float y;
float z;
} element;
struct {
float w;
float x;
float y;
float z;
} element;
} FusionQuaternion;
/**
@@ -49,19 +49,19 @@ typedef union {
* See http://en.wikipedia.org/wiki/Row-major_order
*/
typedef union {
float array[3][3];
float array[3][3];
struct {
float xx;
float xy;
float xz;
float yx;
float yy;
float yz;
float zx;
float zy;
float zz;
} element;
struct {
float xx;
float xy;
float xz;
float yx;
float yy;
float yz;
float zx;
float zy;
float zz;
} element;
} FusionMatrix;
/**
@@ -69,13 +69,13 @@ typedef union {
* X, Y, and Z respectively.
*/
typedef union {
float array[3];
float array[3];
struct {
float roll;
float pitch;
float yaw;
} angle;
struct {
float roll;
float pitch;
float yaw;
} angle;
} FusionEuler;
/**
@@ -124,20 +124,14 @@ typedef union {
* @param degrees Degrees.
* @return Radians.
*/
static inline float FusionDegreesToRadians(const float degrees)
{
return degrees * ((float)M_PI / 180.0f);
}
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);
}
static inline float FusionRadiansToDegrees(const float radians) { return radians * (180.0f / (float)M_PI); }
//------------------------------------------------------------------------------
// Inline functions - Arc sine
@@ -147,15 +141,14 @@ static inline float FusionRadiansToDegrees(const float radians)
* @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);
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);
}
//------------------------------------------------------------------------------
@@ -169,17 +162,16 @@ static inline float FusionAsin(const float value)
* @param x Operand.
* @return Reciprocal of the square root of x.
*/
static inline float FusionFastInverseSqrt(const float x)
{
static inline float FusionFastInverseSqrt(const float x) {
typedef union {
float f;
int32_t i;
} Union32;
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);
Union32 union32 = {.f = x};
union32.i = 0x5F1F1412 - (union32.i >> 1);
return union32.f * (1.69000231f - 0.714158168f * x * union32.f * union32.f);
}
#endif
@@ -192,9 +184,8 @@ static inline float FusionFastInverseSqrt(const float x)
* @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);
static inline bool FusionVectorIsZero(const FusionVector vector) {
return (vector.axis.x == 0.0f) && (vector.axis.y == 0.0f) && (vector.axis.z == 0.0f);
}
/**
@@ -203,14 +194,13 @@ static inline bool FusionVectorIsZero(const FusionVector vector)
* @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;
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;
}
/**
@@ -219,14 +209,13 @@ static inline FusionVector FusionVectorAdd(const FusionVector vectorA, const Fus
* @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;
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;
}
/**
@@ -234,10 +223,7 @@ static inline FusionVector FusionVectorSubtract(const FusionVector vectorA, cons
* @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;
}
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.
@@ -245,14 +231,13 @@ static inline float FusionVectorSum(const FusionVector 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;
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;
}
/**
@@ -261,14 +246,13 @@ static inline FusionVector FusionVectorMultiplyScalar(const FusionVector vector,
* @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;
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;
}
/**
@@ -277,16 +261,15 @@ static inline FusionVector FusionVectorHadamardProduct(const FusionVector vector
* @param vectorB Vector B.
* @return Cross product.
*/
static inline FusionVector FusionVectorCrossProduct(const FusionVector vectorA, const FusionVector vectorB)
{
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;
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
}
@@ -297,9 +280,8 @@ static inline FusionVector FusionVectorCrossProduct(const FusionVector vectorA,
* @param vectorB Vector B.
* @return Dot product.
*/
static inline float FusionVectorDotProduct(const FusionVector vectorA, const FusionVector vectorB)
{
return FusionVectorSum(FusionVectorHadamardProduct(vectorA, vectorB));
static inline float FusionVectorDotProduct(const FusionVector vectorA, const FusionVector vectorB) {
return FusionVectorSum(FusionVectorHadamardProduct(vectorA, vectorB));
}
/**
@@ -307,34 +289,27 @@ static inline float FusionVectorDotProduct(const FusionVector vectorA, const Fus
* @param vector Vector.
* @return Vector magnitude squared.
*/
static inline float FusionVectorMagnitudeSquared(const FusionVector vector)
{
return FusionVectorSum(FusionVectorHadamardProduct(vector, vector));
}
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));
}
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)
{
static inline FusionVector FusionVectorNormalise(const FusionVector vector) {
#ifdef FUSION_USE_NORMAL_SQRT
const float magnitudeReciprocal = 1.0f / sqrtf(FusionVectorMagnitudeSquared(vector));
const float magnitudeReciprocal = 1.0f / sqrtf(FusionVectorMagnitudeSquared(vector));
#else
const float magnitudeReciprocal = FusionFastInverseSqrt(FusionVectorMagnitudeSquared(vector));
const float magnitudeReciprocal = FusionFastInverseSqrt(FusionVectorMagnitudeSquared(vector));
#endif
return FusionVectorMultiplyScalar(vector, magnitudeReciprocal);
return FusionVectorMultiplyScalar(vector, magnitudeReciprocal);
}
//------------------------------------------------------------------------------
@@ -346,15 +321,14 @@ static inline FusionVector FusionVectorNormalise(const FusionVector vector)
* @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;
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;
}
/**
@@ -363,17 +337,16 @@ static inline FusionQuaternion FusionQuaternionAdd(const FusionQuaternion quater
* @param quaternionB Quaternion B (to be pre-multiplied).
* @return Multiplication of two quaternions.
*/
static inline FusionQuaternion FusionQuaternionMultiply(const FusionQuaternion quaternionA, const FusionQuaternion quaternionB)
{
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;
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
}
@@ -387,17 +360,16 @@ static inline FusionQuaternion FusionQuaternionMultiply(const FusionQuaternion q
* @param vector Vector.
* @return Multiplication of a quaternion with a vector.
*/
static inline FusionQuaternion FusionQuaternionMultiplyVector(const FusionQuaternion quaternion, const FusionVector 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;
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
}
@@ -407,21 +379,20 @@ static inline FusionQuaternion FusionQuaternionMultiplyVector(const FusionQuater
* @param quaternion Quaternion.
* @return Normalised quaternion.
*/
static inline FusionQuaternion FusionQuaternionNormalise(const FusionQuaternion 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);
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);
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;
const FusionQuaternion result = {.element = {
.w = Q.w * magnitudeReciprocal,
.x = Q.x * magnitudeReciprocal,
.y = Q.y * magnitudeReciprocal,
.z = Q.z * magnitudeReciprocal,
}};
return result;
#undef Q
}
@@ -434,15 +405,14 @@ static inline FusionQuaternion FusionQuaternionNormalise(const FusionQuaternion
* @param vector Vector.
* @return Multiplication of a matrix with a vector.
*/
static inline FusionVector FusionMatrixMultiplyVector(const FusionMatrix matrix, const FusionVector 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;
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
}
@@ -454,28 +424,27 @@ static inline FusionVector FusionMatrixMultiplyVector(const FusionMatrix matrix,
* @param quaternion Quaternion.
* @return Rotation matrix.
*/
static inline FusionMatrix FusionQuaternionToMatrix(const FusionQuaternion quaternion)
{
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;
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
}
@@ -484,16 +453,15 @@ static inline FusionMatrix FusionQuaternionToMatrix(const FusionQuaternion quate
* @param quaternion Quaternion.
* @return Euler angles in degrees.
*/
static inline FusionEuler FusionQuaternionToEuler(const FusionQuaternion quaternion)
{
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;
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
}

47
src/Fusion/FusionOffset.c
View File

@@ -37,12 +37,11 @@
* @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;
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;
}
/**
@@ -52,28 +51,26 @@ void FusionOffsetInitialise(FusionOffset *const offset, const unsigned int sampl
* @param gyroscope Gyroscope measurement in degrees per second.
* @return Corrected gyroscope measurement in degrees per second.
*/
FusionVector FusionOffsetUpdate(FusionOffset *const offset, FusionVector gyroscope)
{
FusionVector FusionOffsetUpdate(FusionOffset *const offset, FusionVector gyroscope) {
// Subtract offset from gyroscope measurement
gyroscope = FusionVectorSubtract(gyroscope, offset->gyroscopeOffset);
// 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));
// 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;
}
//------------------------------------------------------------------------------

8
src/Fusion/FusionOffset.h
View File

@@ -21,10 +21,10 @@
* internally and must not be accessed by the application.
*/
typedef struct {
float filterCoefficient;
unsigned int timeout;
unsigned int timer;
FusionVector gyroscopeOffset;
float filterCoefficient;
unsigned int timeout;
unsigned int timer;
FusionVector gyroscopeOffset;
} FusionOffset;
//------------------------------------------------------------------------------