mirror
/
meshtastic-firmware
kopia lustrzana https://github.com/meshtastic/firmware
1
0
Forkuj 1
Kod Zgłoszenia Packages Projekty Wydania Wiki Aktywność

Add support for BMX160/RAK12034 compass module (#4021)

pull/4079/head
Jonathan Bennett 2024-06-11 17:47:45 -05:00 zatwierdzone przez GitHub
rodzic 7f2647abb1
commit 0852a170a3
Nie znaleziono w bazie danych klucza dla tego podpisu
ID klucza GPG: B5690EEEBB952194
22 zmienionych plików z 1760 dodań i 10 usunięć
Pokaż statystyki Ściągnij plik aktualizacji Ściągnij plik porównania Expand all files Collapse all files

81
src/AccelerometerThread.h
Wyświetl plik

@ -14,6 +14,10 @@
#include <Arduino.h>
#include <SensorBMA423.hpp>
#include <Wire.h>
#ifdef RAK_4631
#include "Fusion/Fusion.h"
#include <Rak_BMX160.h>
#endif
#define ACCELEROMETER_CHECK_INTERVAL_MS 100
#define ACCELEROMETER_CLICK_THRESHOLD 40
@ -50,12 +54,13 @@ class AccelerometerThread : public concurrency::OSThread
return;
}
acceleremoter_type = type;
#ifndef RAK_4631
if (!config.display.wake_on_tap_or_motion && !config.device.double_tap_as_button_press) {
LOG_DEBUG("AccelerometerThread disabling due to no interested configurations\n");
disable();
return;
}
#endif
init();
}
@ -87,6 +92,71 @@ class AccelerometerThread : public concurrency::OSThread
wakeScreen();
return 500;
}
#ifdef RAK_4631
} else if (acceleremoter_type == ScanI2C::DeviceType::BMX160) {
sBmx160SensorData_t magAccel;
sBmx160SensorData_t gAccel;
/* Get a new sensor event */
bmx160.getAllData(&magAccel, NULL, &gAccel);
// expirimental calibrate routine. Limited to between 10 and 30 seconds after boot
if (millis() > 10 * 1000 && millis() < 30 * 1000) {
if (magAccel.x > highestX)
highestX = magAccel.x;
if (magAccel.x < lowestX)
lowestX = magAccel.x;
if (magAccel.y > highestY)
highestY = magAccel.y;
if (magAccel.y < lowestY)
lowestY = magAccel.y;
if (magAccel.z > highestZ)
highestZ = magAccel.z;
if (magAccel.z < lowestZ)
lowestZ = magAccel.z;
}
int highestRealX = highestX - (highestX + lowestX) / 2;
magAccel.x -= (highestX + lowestX) / 2;
magAccel.y -= (highestY + lowestY) / 2;
magAccel.z -= (highestZ + lowestZ) / 2;
FusionVector ga, ma;
ga.axis.x = -gAccel.x; // default location for the BMX160 is on the rear of the board
ga.axis.y = -gAccel.y;
ga.axis.z = gAccel.z;
ma.axis.x = -magAccel.x;
ma.axis.y = -magAccel.y;
ma.axis.z = magAccel.z * 3;
// If we're set to one of the inverted positions
if (config.display.compass_orientation > meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270) {
ma = FusionAxesSwap(ma, FusionAxesAlignmentNXNYPZ);
ga = FusionAxesSwap(ga, FusionAxesAlignmentNXNYPZ);
}
float heading = FusionCompassCalculateHeading(FusionConventionNed, ga, ma);
switch (config.display.compass_orientation) {
case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_0:
break;
case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_90:
case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_90_INVERTED:
heading += 90;
break;
case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_180:
case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_180_INVERTED:
heading += 180;
break;
case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270:
case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270_INVERTED:
heading += 270;
break;
}
screen->setHeading(heading);
#endif
} else if (acceleremoter_type == ScanI2C::DeviceType::LSM6DS3 && lsm.shake()) {
wakeScreen();
return 500;
@ -149,6 +219,11 @@ class AccelerometerThread : public concurrency::OSThread
bmaSensor.enableTiltIRQ();
// It corresponds to isDoubleClick interrupt
bmaSensor.enableWakeupIRQ();
#ifdef RAK_4631
} else if (acceleremoter_type == ScanI2C::DeviceType::BMX160 && bmx160.begin()) {
bmx160.ODR_Config(BMX160_ACCEL_ODR_100HZ, BMX160_GYRO_ODR_100HZ); // set output data rate
#endif
} else if (acceleremoter_type == ScanI2C::DeviceType::LSM6DS3 && lsm.begin_I2C(accelerometer_found.address)) {
LOG_DEBUG("LSM6DS3 initializing\n");
// Default threshold of 2G, less sensitive options are 4, 8 or 16G
@ -179,6 +254,10 @@ class AccelerometerThread : public concurrency::OSThread
Adafruit_LIS3DH lis;
Adafruit_LSM6DS3TRC lsm;
SensorBMA423 bmaSensor;
#ifdef RAK_4631
RAK_BMX160 bmx160;
float highestX = 0, lowestX = 0, highestY = 0, lowestY = 0, highestZ = 0, lowestZ = 0;
#endif
bool BMA_IRQ = false;
};

32
src/Fusion/Fusion.h 100644
Wyświetl plik

@ -0,0 +1,32 @@
/**
* @file Fusion.h
* @author Seb Madgwick
* @brief Main header file for the Fusion library. This is the only file that
* needs to be included when using the library.
*/
#ifndef FUSION_H
#define FUSION_H
//------------------------------------------------------------------------------
// Includes
#ifdef __cplusplus
extern "C" {
#endif
#include "FusionAhrs.h"
#include "FusionAxes.h"
#include "FusionCalibration.h"
#include "FusionCompass.h"
#include "FusionConvention.h"
#include "FusionMath.h"
#include "FusionOffset.h"
#ifdef __cplusplus
}
#endif
#endif
//------------------------------------------------------------------------------
// End of file

542
src/Fusion/FusionAhrs.c 100644
Wyświetl plik

@ -0,0 +1,542 @@
/**
* @file FusionAhrs.c
* @author Seb Madgwick
* @brief AHRS algorithm to combine gyroscope, accelerometer, and magnetometer
* measurements into a single measurement of orientation relative to the Earth.
*/
//------------------------------------------------------------------------------
// Includes
#include "FusionAhrs.h"
#include <float.h> // FLT_MAX
#include <math.h> // atan2f, cosf, fabsf, powf, sinf
//------------------------------------------------------------------------------
// Definitions
/**
* @brief Initial gain used during the initialisation.
*/
#define INITIAL_GAIN (10.0f)
/**
* @brief Initialisation period in seconds.
*/
#define INITIALISATION_PERIOD (3.0f)
//------------------------------------------------------------------------------
// Function declarations
static inline FusionVector HalfGravity(const FusionAhrs *const ahrs);
static inline FusionVector HalfMagnetic(const FusionAhrs *const ahrs);
static inline FusionVector Feedback(const FusionVector sensor, const FusionVector reference);
static inline int Clamp(const int value, const int min, const int max);
//------------------------------------------------------------------------------
// Functions
/**
* @brief Initialises the AHRS algorithm structure.
* @param ahrs AHRS algorithm structure.
*/
void FusionAhrsInitialise(FusionAhrs *const ahrs)
{
const FusionAhrsSettings settings = {
.convention = FusionConventionNwu,
.gain = 0.5f,
.gyroscopeRange = 0.0f,
.accelerationRejection = 90.0f,
.magneticRejection = 90.0f,
.recoveryTriggerPeriod = 0,
};
FusionAhrsSetSettings(ahrs, &settings);
FusionAhrsReset(ahrs);
}
/**
* @brief Resets the AHRS algorithm. This is equivalent to reinitialising the
* algorithm while maintaining the current settings.
* @param ahrs AHRS algorithm structure.
*/
void FusionAhrsReset(FusionAhrs *const ahrs)
{
ahrs->quaternion = FUSION_IDENTITY_QUATERNION;
ahrs->accelerometer = FUSION_VECTOR_ZERO;
ahrs->initialising = true;
ahrs->rampedGain = INITIAL_GAIN;
ahrs->angularRateRecovery = false;
ahrs->halfAccelerometerFeedback = FUSION_VECTOR_ZERO;
ahrs->halfMagnetometerFeedback = FUSION_VECTOR_ZERO;
ahrs->accelerometerIgnored = false;
ahrs->accelerationRecoveryTrigger = 0;
ahrs->accelerationRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
ahrs->magnetometerIgnored = false;
ahrs->magneticRecoveryTrigger = 0;
ahrs->magneticRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
}
/**
* @brief Sets the AHRS algorithm settings.
* @param ahrs AHRS algorithm structure.
* @param settings Settings.
*/
void FusionAhrsSetSettings(FusionAhrs *const ahrs, const FusionAhrsSettings *const settings)
{
ahrs->settings.convention = settings->convention;
ahrs->settings.gain = settings->gain;
ahrs->settings.gyroscopeRange = settings->gyroscopeRange == 0.0f ? FLT_MAX : 0.98f * settings->gyroscopeRange;
ahrs->settings.accelerationRejection = settings->accelerationRejection == 0.0f
? FLT_MAX
: powf(0.5f * sinf(FusionDegreesToRadians(settings->accelerationRejection)), 2);
ahrs->settings.magneticRejection =
settings->magneticRejection == 0.0f ? FLT_MAX : powf(0.5f * sinf(FusionDegreesToRadians(settings->magneticRejection)), 2);
ahrs->settings.recoveryTriggerPeriod = settings->recoveryTriggerPeriod;
ahrs->accelerationRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
ahrs->magneticRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
if ((settings->gain == 0.0f) ||
(settings->recoveryTriggerPeriod == 0)) { // disable acceleration and magnetic rejection features if gain is zero
ahrs->settings.accelerationRejection = FLT_MAX;
ahrs->settings.magneticRejection = FLT_MAX;
}
if (ahrs->initialising == false) {
ahrs->rampedGain = ahrs->settings.gain;
}
ahrs->rampedGainStep = (INITIAL_GAIN - ahrs->settings.gain) / INITIALISATION_PERIOD;
}
/**
* @brief Updates the AHRS algorithm using the gyroscope, accelerometer, and
* magnetometer measurements.
* @param ahrs AHRS algorithm structure.
* @param gyroscope Gyroscope measurement in degrees per second.
* @param accelerometer Accelerometer measurement in g.
* @param magnetometer Magnetometer measurement in arbitrary units.
* @param deltaTime Delta time in seconds.
*/
void FusionAhrsUpdate(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer,
const FusionVector magnetometer, const float deltaTime)
{
#define Q ahrs->quaternion.element
// Store accelerometer
ahrs->accelerometer = accelerometer;
// Reinitialise if gyroscope range exceeded
if ((fabsf(gyroscope.axis.x) > ahrs->settings.gyroscopeRange) || (fabsf(gyroscope.axis.y) > ahrs->settings.gyroscopeRange) ||
(fabsf(gyroscope.axis.z) > ahrs->settings.gyroscopeRange)) {
const FusionQuaternion quaternion = ahrs->quaternion;
FusionAhrsReset(ahrs);
ahrs->quaternion = quaternion;
ahrs->angularRateRecovery = true;
}
// Ramp down gain during initialisation
if (ahrs->initialising) {
ahrs->rampedGain -= ahrs->rampedGainStep * deltaTime;
if ((ahrs->rampedGain < ahrs->settings.gain) || (ahrs->settings.gain == 0.0f)) {
ahrs->rampedGain = ahrs->settings.gain;
ahrs->initialising = false;
ahrs->angularRateRecovery = false;
}
}
// Calculate direction of gravity indicated by algorithm
const FusionVector halfGravity = HalfGravity(ahrs);
// Calculate accelerometer feedback
FusionVector halfAccelerometerFeedback = FUSION_VECTOR_ZERO;
ahrs->accelerometerIgnored = true;
if (FusionVectorIsZero(accelerometer) == false) {
// Calculate accelerometer feedback scaled by 0.5
ahrs->halfAccelerometerFeedback = Feedback(FusionVectorNormalise(accelerometer), halfGravity);
// Don't ignore accelerometer if acceleration error below threshold
if (ahrs->initialising ||
((FusionVectorMagnitudeSquared(ahrs->halfAccelerometerFeedback) <= ahrs->settings.accelerationRejection))) {
ahrs->accelerometerIgnored = false;
ahrs->accelerationRecoveryTrigger -= 9;
} else {
ahrs->accelerationRecoveryTrigger += 1;
}
// Don't ignore accelerometer during acceleration recovery
if (ahrs->accelerationRecoveryTrigger > ahrs->accelerationRecoveryTimeout) {
ahrs->accelerationRecoveryTimeout = 0;
ahrs->accelerometerIgnored = false;
} else {
ahrs->accelerationRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
}
ahrs->accelerationRecoveryTrigger = Clamp(ahrs->accelerationRecoveryTrigger, 0, ahrs->settings.recoveryTriggerPeriod);
// Apply accelerometer feedback
if (ahrs->accelerometerIgnored == false) {
halfAccelerometerFeedback = ahrs->halfAccelerometerFeedback;
}
}
// Calculate magnetometer feedback
FusionVector halfMagnetometerFeedback = FUSION_VECTOR_ZERO;
ahrs->magnetometerIgnored = true;
if (FusionVectorIsZero(magnetometer) == false) {
// Calculate direction of magnetic field indicated by algorithm
const FusionVector halfMagnetic = HalfMagnetic(ahrs);
// Calculate magnetometer feedback scaled by 0.5
ahrs->halfMagnetometerFeedback =
Feedback(FusionVectorNormalise(FusionVectorCrossProduct(halfGravity, magnetometer)), halfMagnetic);
// Don't ignore magnetometer if magnetic error below threshold
if (ahrs->initialising ||
((FusionVectorMagnitudeSquared(ahrs->halfMagnetometerFeedback) <= ahrs->settings.magneticRejection))) {
ahrs->magnetometerIgnored = false;
ahrs->magneticRecoveryTrigger -= 9;
} else {
ahrs->magneticRecoveryTrigger += 1;
}
// Don't ignore magnetometer during magnetic recovery
if (ahrs->magneticRecoveryTrigger > ahrs->magneticRecoveryTimeout) {
ahrs->magneticRecoveryTimeout = 0;
ahrs->magnetometerIgnored = false;
} else {
ahrs->magneticRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
}
ahrs->magneticRecoveryTrigger = Clamp(ahrs->magneticRecoveryTrigger, 0, ahrs->settings.recoveryTriggerPeriod);
// Apply magnetometer feedback
if (ahrs->magnetometerIgnored == false) {
halfMagnetometerFeedback = ahrs->halfMagnetometerFeedback;
}
}
// Convert gyroscope to radians per second scaled by 0.5
const FusionVector halfGyroscope = FusionVectorMultiplyScalar(gyroscope, FusionDegreesToRadians(0.5f));
// Apply feedback to gyroscope
const FusionVector adjustedHalfGyroscope = FusionVectorAdd(
halfGyroscope,
FusionVectorMultiplyScalar(FusionVectorAdd(halfAccelerometerFeedback, halfMagnetometerFeedback), ahrs->rampedGain));
// Integrate rate of change of quaternion
ahrs->quaternion = FusionQuaternionAdd(
ahrs->quaternion,
FusionQuaternionMultiplyVector(ahrs->quaternion, FusionVectorMultiplyScalar(adjustedHalfGyroscope, deltaTime)));
// Normalise quaternion
ahrs->quaternion = FusionQuaternionNormalise(ahrs->quaternion);
#undef Q
}
/**
* @brief Returns the direction of gravity scaled by 0.5.
* @param ahrs AHRS algorithm structure.
* @return Direction of gravity scaled by 0.5.
*/
static inline FusionVector HalfGravity(const FusionAhrs *const ahrs)
{
#define Q ahrs->quaternion.element
switch (ahrs->settings.convention) {
case FusionConventionNwu:
case FusionConventionEnu: {
const FusionVector halfGravity = {.axis = {
.x = Q.x * Q.z - Q.w * Q.y,
.y = Q.y * Q.z + Q.w * Q.x,
.z = Q.w * Q.w - 0.5f + Q.z * Q.z,
}}; // third column of transposed rotation matrix scaled by 0.5
return halfGravity;
}
case FusionConventionNed: {
const FusionVector halfGravity = {.axis = {
.x = Q.w * Q.y - Q.x * Q.z,
.y = -1.0f * (Q.y * Q.z + Q.w * Q.x),
.z = 0.5f - Q.w * Q.w - Q.z * Q.z,
}}; // third column of transposed rotation matrix scaled by -0.5
return halfGravity;
}
}
return FUSION_VECTOR_ZERO; // avoid compiler warning
#undef Q
}
/**
* @brief Returns the direction of the magnetic field scaled by 0.5.
* @param ahrs AHRS algorithm structure.
* @return Direction of the magnetic field scaled by 0.5.
*/
static inline FusionVector HalfMagnetic(const FusionAhrs *const ahrs)
{
#define Q ahrs->quaternion.element
switch (ahrs->settings.convention) {
case FusionConventionNwu: {
const FusionVector halfMagnetic = {.axis = {
.x = Q.x * Q.y + Q.w * Q.z,
.y = Q.w * Q.w - 0.5f + Q.y * Q.y,
.z = Q.y * Q.z - Q.w * Q.x,
}}; // second column of transposed rotation matrix scaled by 0.5
return halfMagnetic;
}
case FusionConventionEnu: {
const FusionVector halfMagnetic = {.axis = {
.x = 0.5f - Q.w * Q.w - Q.x * Q.x,
.y = Q.w * Q.z - Q.x * Q.y,
.z = -1.0f * (Q.x * Q.z + Q.w * Q.y),
}}; // first column of transposed rotation matrix scaled by -0.5
return halfMagnetic;
}
case FusionConventionNed: {
const FusionVector halfMagnetic = {.axis = {
.x = -1.0f * (Q.x * Q.y + Q.w * Q.z),
.y = 0.5f - Q.w * Q.w - Q.y * Q.y,
.z = Q.w * Q.x - Q.y * Q.z,
}}; // second column of transposed rotation matrix scaled by -0.5
return halfMagnetic;
}
}
return FUSION_VECTOR_ZERO; // avoid compiler warning
#undef Q
}
/**
* @brief Returns the feedback.
* @param sensor Sensor.
* @param reference Reference.
* @return Feedback.
*/
static inline FusionVector Feedback(const FusionVector sensor, const FusionVector reference)
{
if (FusionVectorDotProduct(sensor, reference) < 0.0f) { // if error is >90 degrees
return FusionVectorNormalise(FusionVectorCrossProduct(sensor, reference));
}
return FusionVectorCrossProduct(sensor, reference);
}
/**
* @brief Returns a value limited to maximum and minimum.
* @param value Value.
* @param min Minimum value.
* @param max Maximum value.
* @return Value limited to maximum and minimum.
*/
static inline int Clamp(const int value, const int min, const int max)
{
if (value < min) {
return min;
}
if (value > max) {
return max;
}
return value;
}
/**
* @brief Updates the AHRS algorithm using the gyroscope and accelerometer
* measurements only.
* @param ahrs AHRS algorithm structure.
* @param gyroscope Gyroscope measurement in degrees per second.
* @param accelerometer Accelerometer measurement in g.
* @param deltaTime Delta time in seconds.
*/
void FusionAhrsUpdateNoMagnetometer(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer,
const float deltaTime)
{
// Update AHRS algorithm
FusionAhrsUpdate(ahrs, gyroscope, accelerometer, FUSION_VECTOR_ZERO, deltaTime);
// Zero heading during initialisation
if (ahrs->initialising) {
FusionAhrsSetHeading(ahrs, 0.0f);
}
}
/**
* @brief Updates the AHRS algorithm using the gyroscope, accelerometer, and
* heading measurements.
* @param ahrs AHRS algorithm structure.
* @param gyroscope Gyroscope measurement in degrees per second.
* @param accelerometer Accelerometer measurement in g.
* @param heading Heading measurement in degrees.
* @param deltaTime Delta time in seconds.
*/
void FusionAhrsUpdateExternalHeading(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer,
const float heading, const float deltaTime)
{
#define Q ahrs->quaternion.element
// Calculate roll
const float roll = atan2f(Q.w * Q.x + Q.y * Q.z, 0.5f - Q.y * Q.y - Q.x * Q.x);
// Calculate magnetometer
const float headingRadians = FusionDegreesToRadians(heading);
const float sinHeadingRadians = sinf(headingRadians);
const FusionVector magnetometer = {.axis = {
.x = cosf(headingRadians),
.y = -1.0f * cosf(roll) * sinHeadingRadians,
.z = sinHeadingRadians * sinf(roll),
}};
// Update AHRS algorithm
FusionAhrsUpdate(ahrs, gyroscope, accelerometer, magnetometer, deltaTime);
#undef Q
}
/**
* @brief Returns the quaternion describing the sensor relative to the Earth.
* @param ahrs AHRS algorithm structure.
* @return Quaternion describing the sensor relative to the Earth.
*/
FusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs *const ahrs)
{
return ahrs->quaternion;
}
/**
* @brief Sets the quaternion describing the sensor relative to the Earth.
* @param ahrs AHRS algorithm structure.
* @param quaternion Quaternion describing the sensor relative to the Earth.
*/
void FusionAhrsSetQuaternion(FusionAhrs *const ahrs, const FusionQuaternion quaternion)
{
ahrs->quaternion = quaternion;
}
/**
* @brief Returns the linear acceleration measurement equal to the accelerometer
* measurement with the 1 g of gravity removed.
* @param ahrs AHRS algorithm structure.
* @return Linear acceleration measurement in g.
*/
FusionVector FusionAhrsGetLinearAcceleration(const FusionAhrs *const ahrs)
{
#define Q ahrs->quaternion.element
// Calculate gravity in the sensor coordinate frame
const FusionVector gravity = {.axis = {
.x = 2.0f * (Q.x * Q.z - Q.w * Q.y),
.y = 2.0f * (Q.y * Q.z + Q.w * Q.x),
.z = 2.0f * (Q.w * Q.w - 0.5f + Q.z * Q.z),
}}; // third column of transposed rotation matrix
// Remove gravity from accelerometer measurement
switch (ahrs->settings.convention) {
case FusionConventionNwu:
case FusionConventionEnu: {
return FusionVectorSubtract(ahrs->accelerometer, gravity);
}
case FusionConventionNed: {
return FusionVectorAdd(ahrs->accelerometer, gravity);
}
}
return FUSION_VECTOR_ZERO; // avoid compiler warning
#undef Q
}
/**
* @brief Returns the Earth acceleration measurement equal to accelerometer
* measurement in the Earth coordinate frame with the 1 g of gravity removed.
* @param ahrs AHRS algorithm structure.
* @return Earth acceleration measurement in g.
*/
FusionVector FusionAhrsGetEarthAcceleration(const FusionAhrs *const ahrs)
{
#define Q ahrs->quaternion.element
#define A ahrs->accelerometer.axis
// Calculate accelerometer measurement in the Earth coordinate frame
const float qwqw = Q.w * Q.w; // calculate common terms to avoid repeated operations
const float qwqx = Q.w * Q.x;
const float qwqy = Q.w * Q.y;
const float qwqz = Q.w * Q.z;
const float qxqy = Q.x * Q.y;
const float qxqz = Q.x * Q.z;
const float qyqz = Q.y * Q.z;
FusionVector accelerometer = {.axis = {
.x = 2.0f * ((qwqw - 0.5f + Q.x * Q.x) * A.x + (qxqy - qwqz) * A.y + (qxqz + qwqy) * A.z),
.y = 2.0f * ((qxqy + qwqz) * A.x + (qwqw - 0.5f + Q.y * Q.y) * A.y + (qyqz - qwqx) * A.z),
.z = 2.0f * ((qxqz - qwqy) * A.x + (qyqz + qwqx) * A.y + (qwqw - 0.5f + Q.z * Q.z) * A.z),
}}; // rotation matrix multiplied with the accelerometer
// Remove gravity from accelerometer measurement
switch (ahrs->settings.convention) {
case FusionConventionNwu:
case FusionConventionEnu:
accelerometer.axis.z -= 1.0f;
break;
case FusionConventionNed:
accelerometer.axis.z += 1.0f;
break;
}
return accelerometer;
#undef Q
#undef A
}
/**
* @brief Returns the AHRS algorithm internal states.
* @param ahrs AHRS algorithm structure.
* @return AHRS algorithm internal states.
*/
FusionAhrsInternalStates FusionAhrsGetInternalStates(const FusionAhrs *const ahrs)
{
const FusionAhrsInternalStates internalStates = {
.accelerationError = FusionRadiansToDegrees(FusionAsin(2.0f * FusionVectorMagnitude(ahrs->halfAccelerometerFeedback))),
.accelerometerIgnored = ahrs->accelerometerIgnored,
.accelerationRecoveryTrigger =
ahrs->settings.recoveryTriggerPeriod == 0
? 0.0f
: (float)ahrs->accelerationRecoveryTrigger / (float)ahrs->settings.recoveryTriggerPeriod,
.magneticError = FusionRadiansToDegrees(FusionAsin(2.0f * FusionVectorMagnitude(ahrs->halfMagnetometerFeedback))),
.magnetometerIgnored = ahrs->magnetometerIgnored,
.magneticRecoveryTrigger = ahrs->settings.recoveryTriggerPeriod == 0
? 0.0f
: (float)ahrs->magneticRecoveryTrigger / (float)ahrs->settings.recoveryTriggerPeriod,
};
return internalStates;
}
/**
* @brief Returns the AHRS algorithm flags.
* @param ahrs AHRS algorithm structure.
* @return AHRS algorithm flags.
*/
FusionAhrsFlags FusionAhrsGetFlags(const FusionAhrs *const ahrs)
{
const FusionAhrsFlags flags = {
.initialising = ahrs->initialising,
.angularRateRecovery = ahrs->angularRateRecovery,
.accelerationRecovery = ahrs->accelerationRecoveryTrigger > ahrs->accelerationRecoveryTimeout,
.magneticRecovery = ahrs->magneticRecoveryTrigger > ahrs->magneticRecoveryTimeout,
};
return flags;
}
/**
* @brief Sets the heading of the orientation measurement provided by the AHRS
* algorithm. This function can be used to reset drift in heading when the AHRS
* algorithm is being used without a magnetometer.
* @param ahrs AHRS algorithm structure.
* @param heading Heading angle in degrees.
*/
void FusionAhrsSetHeading(FusionAhrs *const ahrs, const float heading)
{
#define Q ahrs->quaternion.element
const float yaw = atan2f(Q.w * Q.z + Q.x * Q.y, 0.5f - Q.y * Q.y - Q.z * Q.z);
const float halfYawMinusHeading = 0.5f * (yaw - FusionDegreesToRadians(heading));
const FusionQuaternion rotation = {.element = {
.w = cosf(halfYawMinusHeading),
.x = 0.0f,
.y = 0.0f,
.z = -1.0f * sinf(halfYawMinusHeading),
}};
ahrs->quaternion = FusionQuaternionMultiply(rotation, ahrs->quaternion);
#undef Q
}
//------------------------------------------------------------------------------
// End of file

112
src/Fusion/FusionAhrs.h 100644
Wyświetl plik

@ -0,0 +1,112 @@
/**
* @file FusionAhrs.h
* @author Seb Madgwick
* @brief AHRS algorithm to combine gyroscope, accelerometer, and magnetometer
* measurements into a single measurement of orientation relative to the Earth.
*/
#ifndef FUSION_AHRS_H
#define FUSION_AHRS_H
//------------------------------------------------------------------------------
// Includes
#include "FusionConvention.h"
#include "FusionMath.h"
#include <stdbool.h>
//------------------------------------------------------------------------------
// Definitions
/**
* @brief AHRS algorithm settings.
*/
typedef struct {
FusionConvention convention;
float gain;
float gyroscopeRange;
float accelerationRejection;
float magneticRejection;
unsigned int recoveryTriggerPeriod;
} FusionAhrsSettings;
/**
* @brief AHRS algorithm structure. Structure members are used internally and
* must not be accessed by the application.
*/
typedef struct {
FusionAhrsSettings settings;
FusionQuaternion quaternion;
FusionVector accelerometer;
bool initialising;
float rampedGain;
float rampedGainStep;
bool angularRateRecovery;
FusionVector halfAccelerometerFeedback;
FusionVector halfMagnetometerFeedback;
bool accelerometerIgnored;
int accelerationRecoveryTrigger;
int accelerationRecoveryTimeout;
bool magnetometerIgnored;
int magneticRecoveryTrigger;
int magneticRecoveryTimeout;
} FusionAhrs;
/**
* @brief AHRS algorithm internal states.
*/
typedef struct {
float accelerationError;
bool accelerometerIgnored;
float accelerationRecoveryTrigger;
float magneticError;
bool magnetometerIgnored;
float magneticRecoveryTrigger;
} FusionAhrsInternalStates;
/**
* @brief AHRS algorithm flags.
*/
typedef struct {
bool initialising;
bool angularRateRecovery;
bool accelerationRecovery;
bool magneticRecovery;
} FusionAhrsFlags;
//------------------------------------------------------------------------------
// Function declarations
void FusionAhrsInitialise(FusionAhrs *const ahrs);
void FusionAhrsReset(FusionAhrs *const ahrs);
void FusionAhrsSetSettings(FusionAhrs *const ahrs, const FusionAhrsSettings *const settings);
void FusionAhrsUpdate(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer,
const FusionVector magnetometer, const float deltaTime);
void FusionAhrsUpdateNoMagnetometer(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer,
const float deltaTime);
void FusionAhrsUpdateExternalHeading(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer,
const float heading, const float deltaTime);
FusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs *const ahrs);
void FusionAhrsSetQuaternion(FusionAhrs *const ahrs, const FusionQuaternion quaternion);
FusionVector FusionAhrsGetLinearAcceleration(const FusionAhrs *const ahrs);
FusionVector FusionAhrsGetEarthAcceleration(const FusionAhrs *const ahrs);
FusionAhrsInternalStates FusionAhrsGetInternalStates(const FusionAhrs *const ahrs);
FusionAhrsFlags FusionAhrsGetFlags(const FusionAhrs *const ahrs);
void FusionAhrsSetHeading(FusionAhrs *const ahrs, const float heading);
#endif
//------------------------------------------------------------------------------
// End of file

188
src/Fusion/FusionAxes.h 100644
Wyświetl plik

@ -0,0 +1,188 @@
/**
* @file FusionAxes.h
* @author Seb Madgwick
* @brief Swaps sensor axes for alignment with the body axes.
*/
#ifndef FUSION_AXES_H
#define FUSION_AXES_H
//------------------------------------------------------------------------------
// Includes
#include "FusionMath.h"
//------------------------------------------------------------------------------
// Definitions
/**
* @brief Axes alignment describing the sensor axes relative to the body axes.
* For example, if the body X axis is aligned with the sensor Y axis and the
* body Y axis is aligned with sensor X axis but pointing the opposite direction
* then alignment is +Y-X+Z.
*/
typedef enum {
FusionAxesAlignmentPXPYPZ, /* +X+Y+Z */
FusionAxesAlignmentPXNZPY, /* +X-Z+Y */
FusionAxesAlignmentPXNYNZ, /* +X-Y-Z */
FusionAxesAlignmentPXPZNY, /* +X+Z-Y */
FusionAxesAlignmentNXPYNZ, /* -X+Y-Z */
FusionAxesAlignmentNXPZPY, /* -X+Z+Y */
FusionAxesAlignmentNXNYPZ, /* -X-Y+Z */
FusionAxesAlignmentNXNZNY, /* -X-Z-Y */
FusionAxesAlignmentPYNXPZ, /* +Y-X+Z */
FusionAxesAlignmentPYNZNX, /* +Y-Z-X */
FusionAxesAlignmentPYPXNZ, /* +Y+X-Z */
FusionAxesAlignmentPYPZPX, /* +Y+Z+X */
FusionAxesAlignmentNYPXPZ, /* -Y+X+Z */
FusionAxesAlignmentNYNZPX, /* -Y-Z+X */
FusionAxesAlignmentNYNXNZ, /* -Y-X-Z */
FusionAxesAlignmentNYPZNX, /* -Y+Z-X */
FusionAxesAlignmentPZPYNX, /* +Z+Y-X */
FusionAxesAlignmentPZPXPY, /* +Z+X+Y */
FusionAxesAlignmentPZNYPX, /* +Z-Y+X */
FusionAxesAlignmentPZNXNY, /* +Z-X-Y */
FusionAxesAlignmentNZPYPX, /* -Z+Y+X */
FusionAxesAlignmentNZNXPY, /* -Z-X+Y */
FusionAxesAlignmentNZNYNX, /* -Z-Y-X */
FusionAxesAlignmentNZPXNY, /* -Z+X-Y */
} FusionAxesAlignment;
//------------------------------------------------------------------------------
// Inline functions
/**
* @brief Swaps sensor axes for alignment with the body axes.
* @param sensor Sensor axes.
* @param alignment Axes alignment.
* @return Sensor axes aligned with the body axes.
*/
static inline FusionVector FusionAxesSwap(const FusionVector sensor, const FusionAxesAlignment alignment)
{
FusionVector result;
switch (alignment) {
case FusionAxesAlignmentPXPYPZ:
break;
case FusionAxesAlignmentPXNZPY:
result.axis.x = +sensor.axis.x;
result.axis.y = -sensor.axis.z;
result.axis.z = +sensor.axis.y;
return result;
case FusionAxesAlignmentPXNYNZ:
result.axis.x = +sensor.axis.x;
result.axis.y = -sensor.axis.y;
result.axis.z = -sensor.axis.z;
return result;
case FusionAxesAlignmentPXPZNY:
result.axis.x = +sensor.axis.x;
result.axis.y = +sensor.axis.z;
result.axis.z = -sensor.axis.y;
return result;
case FusionAxesAlignmentNXPYNZ:
result.axis.x = -sensor.axis.x;
result.axis.y = +sensor.axis.y;
result.axis.z = -sensor.axis.z;
return result;
case FusionAxesAlignmentNXPZPY:
result.axis.x = -sensor.axis.x;
result.axis.y = +sensor.axis.z;
result.axis.z = +sensor.axis.y;
return result;
case FusionAxesAlignmentNXNYPZ:
result.axis.x = -sensor.axis.x;
result.axis.y = -sensor.axis.y;
result.axis.z = +sensor.axis.z;
return result;
case FusionAxesAlignmentNXNZNY:
result.axis.x = -sensor.axis.x;
result.axis.y = -sensor.axis.z;
result.axis.z = -sensor.axis.y;
return result;
case FusionAxesAlignmentPYNXPZ:
result.axis.x = +sensor.axis.y;
result.axis.y = -sensor.axis.x;
result.axis.z = +sensor.axis.z;
return result;
case FusionAxesAlignmentPYNZNX:
result.axis.x = +sensor.axis.y;
result.axis.y = -sensor.axis.z;
result.axis.z = -sensor.axis.x;
return result;
case FusionAxesAlignmentPYPXNZ:
result.axis.x = +sensor.axis.y;
result.axis.y = +sensor.axis.x;
result.axis.z = -sensor.axis.z;
return result;
case FusionAxesAlignmentPYPZPX:
result.axis.x = +sensor.axis.y;
result.axis.y = +sensor.axis.z;
result.axis.z = +sensor.axis.x;
return result;
case FusionAxesAlignmentNYPXPZ:
result.axis.x = -sensor.axis.y;
result.axis.y = +sensor.axis.x;
result.axis.z = +sensor.axis.z;
return result;
case FusionAxesAlignmentNYNZPX:
result.axis.x = -sensor.axis.y;
result.axis.y = -sensor.axis.z;
result.axis.z = +sensor.axis.x;
return result;
case FusionAxesAlignmentNYNXNZ:
result.axis.x = -sensor.axis.y;
result.axis.y = -sensor.axis.x;
result.axis.z = -sensor.axis.z;
return result;
case FusionAxesAlignmentNYPZNX:
result.axis.x = -sensor.axis.y;
result.axis.y = +sensor.axis.z;
result.axis.z = -sensor.axis.x;
return result;
case FusionAxesAlignmentPZPYNX:
result.axis.x = +sensor.axis.z;
result.axis.y = +sensor.axis.y;
result.axis.z = -sensor.axis.x;
return result;
case FusionAxesAlignmentPZPXPY:
result.axis.x = +sensor.axis.z;
result.axis.y = +sensor.axis.x;
result.axis.z = +sensor.axis.y;
return result;
case FusionAxesAlignmentPZNYPX:
result.axis.x = +sensor.axis.z;
result.axis.y = -sensor.axis.y;
result.axis.z = +sensor.axis.x;
return result;
case FusionAxesAlignmentPZNXNY:
result.axis.x = +sensor.axis.z;
result.axis.y = -sensor.axis.x;
result.axis.z = -sensor.axis.y;
return result;
case FusionAxesAlignmentNZPYPX:
result.axis.x = -sensor.axis.z;
result.axis.y = +sensor.axis.y;
result.axis.z = +sensor.axis.x;
return result;
case FusionAxesAlignmentNZNXPY:
result.axis.x = -sensor.axis.z;
result.axis.y = -sensor.axis.x;
result.axis.z = +sensor.axis.y;
return result;
case FusionAxesAlignmentNZNYNX:
result.axis.x = -sensor.axis.z;
result.axis.y = -sensor.axis.y;
result.axis.z = -sensor.axis.x;
return result;
case FusionAxesAlignmentNZPXNY:
result.axis.x = -sensor.axis.z;
result.axis.y = +sensor.axis.x;
result.axis.z = -sensor.axis.y;
return result;
}
return sensor; // avoid compiler warning
}
#endif
//------------------------------------------------------------------------------
// End of file

49
src/Fusion/FusionCalibration.h 100644
Wyświetl plik

@ -0,0 +1,49 @@
/**
* @file FusionCalibration.h
* @author Seb Madgwick
* @brief Gyroscope, accelerometer, and magnetometer calibration models.
*/
#ifndef FUSION_CALIBRATION_H
#define FUSION_CALIBRATION_H
//------------------------------------------------------------------------------
// Includes
#include "FusionMath.h"
//------------------------------------------------------------------------------
// Inline functions
/**
* @brief Gyroscope and accelerometer calibration model.
* @param uncalibrated Uncalibrated measurement.
* @param misalignment Misalignment matrix.
* @param sensitivity Sensitivity.
* @param offset Offset.
* @return Calibrated measurement.
*/
static inline FusionVector FusionCalibrationInertial(const FusionVector uncalibrated, const FusionMatrix misalignment,
const FusionVector sensitivity, const FusionVector offset)
{
return FusionMatrixMultiplyVector(misalignment,
FusionVectorHadamardProduct(FusionVectorSubtract(uncalibrated, offset), sensitivity));
}
/**
* @brief Magnetometer calibration model.
* @param uncalibrated Uncalibrated measurement.
* @param softIronMatrix Soft-iron matrix.
* @param hardIronOffset Hard-iron offset.
* @return Calibrated measurement.
*/
static inline FusionVector FusionCalibrationMagnetic(const FusionVector uncalibrated, const FusionMatrix softIronMatrix,
const FusionVector hardIronOffset)
{
return FusionMatrixMultiplyVector(softIronMatrix, FusionVectorSubtract(uncalibrated, hardIronOffset));
}
#endif
//------------------------------------------------------------------------------
// End of file

51
src/Fusion/FusionCompass.c 100644
Wyświetl plik

@ -0,0 +1,51 @@
/**
* @file FusionCompass.c
* @author Seb Madgwick
* @brief Tilt-compensated compass to calculate the magnetic heading using
* accelerometer and magnetometer measurements.
*/
//------------------------------------------------------------------------------
// Includes
#include "FusionCompass.h"
#include "FusionAxes.h"
#include <math.h> // atan2f
//------------------------------------------------------------------------------
// Functions
/**
* @brief Calculates the magnetic heading.
* @param convention Earth axes convention.
* @param accelerometer Accelerometer measurement in any calibrated units.
* @param magnetometer Magnetometer measurement in any calibrated units.
* @return Heading angle in degrees.
*/
float FusionCompassCalculateHeading(const FusionConvention convention, const FusionVector accelerometer,
const FusionVector magnetometer)
{
switch (convention) {
case FusionConventionNwu: {
const FusionVector west = FusionVectorNormalise(FusionVectorCrossProduct(accelerometer, magnetometer));
const FusionVector north = FusionVectorNormalise(FusionVectorCrossProduct(west, accelerometer));
return FusionRadiansToDegrees(atan2f(west.axis.x, north.axis.x));
}
case FusionConventionEnu: {
const FusionVector west = FusionVectorNormalise(FusionVectorCrossProduct(accelerometer, magnetometer));
const FusionVector north = FusionVectorNormalise(FusionVectorCrossProduct(west, accelerometer));
const FusionVector east = FusionVectorMultiplyScalar(west, -1.0f);
return FusionRadiansToDegrees(atan2f(north.axis.x, east.axis.x));
}
case FusionConventionNed: {
const FusionVector up = FusionVectorMultiplyScalar(accelerometer, -1.0f);
const FusionVector west = FusionVectorNormalise(FusionVectorCrossProduct(up, magnetometer));
const FusionVector north = FusionVectorNormalise(FusionVectorCrossProduct(west, up));
return FusionRadiansToDegrees(atan2f(west.axis.x, north.axis.x));
}
}
return 0; // avoid compiler warning
}
//------------------------------------------------------------------------------
// End of file

26
src/Fusion/FusionCompass.h 100644
Wyświetl plik

@ -0,0 +1,26 @@
/**
* @file FusionCompass.h
* @author Seb Madgwick
* @brief Tilt-compensated compass to calculate the magnetic heading using
* accelerometer and magnetometer measurements.
*/
#ifndef FUSION_COMPASS_H
#define FUSION_COMPASS_H
//------------------------------------------------------------------------------
// Includes
#include "FusionConvention.h"
#include "FusionMath.h"
//------------------------------------------------------------------------------
// Function declarations
float FusionCompassCalculateHeading(const FusionConvention convention, const FusionVector accelerometer,
const FusionVector magnetometer);
#endif
//------------------------------------------------------------------------------
// End of file

25
src/Fusion/FusionConvention.h 100644
Wyświetl plik

@ -0,0 +1,25 @@
/**
* @file FusionConvention.h
* @author Seb Madgwick
* @brief Earth axes convention.
*/
#ifndef FUSION_CONVENTION_H
#define FUSION_CONVENTION_H
//------------------------------------------------------------------------------
// Definitions
/**
* @brief Earth axes convention.
*/
typedef enum {
FusionConventionNwu, /* North-West-Up */
FusionConventionEnu, /* East-North-Up */
FusionConventionNed, /* North-East-Down */
} FusionConvention;
#endif
//------------------------------------------------------------------------------
// End of file

503
src/Fusion/FusionMath.h 100644
Wyświetl plik

@ -0,0 +1,503 @@
/**
* @file FusionMath.h
* @author Seb Madgwick
* @brief Math library.
*/
#ifndef FUSION_MATH_H
#define FUSION_MATH_H
//------------------------------------------------------------------------------
// Includes
#include <math.h> // M_PI, sqrtf, atan2f, asinf
#include <stdbool.h>
#include <stdint.h>
//------------------------------------------------------------------------------
// Definitions
/**
* @brief 3D vector.
*/
typedef union {
float array[3];
struct {
float x;
float y;
float z;
} axis;
} FusionVector;
/**
* @brief Quaternion.
*/
typedef union {
float array[4];
struct {
float w;
float x;
float y;
float z;
} element;
} FusionQuaternion;
/**
* @brief 3x3 matrix in row-major order.
* See http://en.wikipedia.org/wiki/Row-major_order
*/
typedef union {
float array[3][3];
struct {
float xx;
float xy;
float xz;
float yx;
float yy;
float yz;
float zx;
float zy;
float zz;
} element;
} FusionMatrix;
/**
* @brief Euler angles. Roll, pitch, and yaw correspond to rotations around
* X, Y, and Z respectively.
*/
typedef union {
float array[3];
struct {
float roll;
float pitch;
float yaw;
} angle;
} FusionEuler;
/**
* @brief Vector of zeros.
*/
#define FUSION_VECTOR_ZERO ((FusionVector){.array = {0.0f, 0.0f, 0.0f}})
/**
* @brief Vector of ones.
*/
#define FUSION_VECTOR_ONES ((FusionVector){.array = {1.0f, 1.0f, 1.0f}})
/**
* @brief Identity quaternion.
*/
#define FUSION_IDENTITY_QUATERNION ((FusionQuaternion){.array = {1.0f, 0.0f, 0.0f, 0.0f}})
/**
* @brief Identity matrix.
*/
#define FUSION_IDENTITY_MATRIX ((FusionMatrix){.array = {{1.0f, 0.0f, 0.0f}, {0.0f, 1.0f, 0.0f}, {0.0f, 0.0f, 1.0f}}})
/**
* @brief Euler angles of zero.
*/
#define FUSION_EULER_ZERO ((FusionEuler){.array = {0.0f, 0.0f, 0.0f}})
/**
* @brief Pi. May not be defined in math.h.
*/
#ifndef M_PI
#define M_PI (3.14159265358979323846)
#endif
/**
* @brief Include this definition or add as a preprocessor definition to use
* normal square root operations.
*/
// #define FUSION_USE_NORMAL_SQRT
//------------------------------------------------------------------------------
// Inline functions - Degrees and radians conversion
/**
* @brief Converts degrees to radians.
* @param degrees Degrees.
* @return Radians.
*/
static inline float FusionDegreesToRadians(const float degrees)
{
return degrees * ((float)M_PI / 180.0f);
}
/**
* @brief Converts radians to degrees.
* @param radians Radians.
* @return Degrees.
*/
static inline float FusionRadiansToDegrees(const float radians)
{
return radians * (180.0f / (float)M_PI);
}
//------------------------------------------------------------------------------
// Inline functions - Arc sine
/**
* @brief Returns the arc sine of the value.
* @param value Value.
* @return Arc sine of the value.
*/
static inline float FusionAsin(const float value)
{
if (value <= -1.0f) {
return (float)M_PI / -2.0f;
}
if (value >= 1.0f) {
return (float)M_PI / 2.0f;
}
return asinf(value);
}
//------------------------------------------------------------------------------
// Inline functions - Fast inverse square root
#ifndef FUSION_USE_NORMAL_SQRT
/**
* @brief Calculates the reciprocal of the square root.
* See https://pizer.wordpress.com/2008/10/12/fast-inverse-square-root/
* @param x Operand.
* @return Reciprocal of the square root of x.
*/
static inline float FusionFastInverseSqrt(const float x)
{
typedef union {
float f;
int32_t i;
} Union32;
Union32 union32 = {.f = x};
union32.i = 0x5F1F1412 - (union32.i >> 1);
return union32.f * (1.69000231f - 0.714158168f * x * union32.f * union32.f);
}
#endif
//------------------------------------------------------------------------------
// Inline functions - Vector operations
/**
* @brief Returns true if the vector is zero.
* @param vector Vector.
* @return True if the vector is zero.
*/
static inline bool FusionVectorIsZero(const FusionVector vector)
{
return (vector.axis.x == 0.0f) && (vector.axis.y == 0.0f) && (vector.axis.z == 0.0f);
}
/**
* @brief Returns the sum of two vectors.
* @param vectorA Vector A.
* @param vectorB Vector B.
* @return Sum of two vectors.
*/
static inline FusionVector FusionVectorAdd(const FusionVector vectorA, const FusionVector vectorB)
{
const FusionVector result = {.axis = {
.x = vectorA.axis.x + vectorB.axis.x,
.y = vectorA.axis.y + vectorB.axis.y,
.z = vectorA.axis.z + vectorB.axis.z,
}};
return result;
}
/**
* @brief Returns vector B subtracted from vector A.
* @param vectorA Vector A.
* @param vectorB Vector B.
* @return Vector B subtracted from vector A.
*/
static inline FusionVector FusionVectorSubtract(const FusionVector vectorA, const FusionVector vectorB)
{
const FusionVector result = {.axis = {
.x = vectorA.axis.x - vectorB.axis.x,
.y = vectorA.axis.y - vectorB.axis.y,
.z = vectorA.axis.z - vectorB.axis.z,
}};
return result;
}
/**
* @brief Returns the sum of the elements.
* @param vector Vector.
* @return Sum of the elements.
*/
static inline float FusionVectorSum(const FusionVector vector)
{
return vector.axis.x + vector.axis.y + vector.axis.z;
}
/**
* @brief Returns the multiplication of a vector by a scalar.
* @param vector Vector.
* @param scalar Scalar.
* @return Multiplication of a vector by a scalar.
*/
static inline FusionVector FusionVectorMultiplyScalar(const FusionVector vector, const float scalar)
{
const FusionVector result = {.axis = {
.x = vector.axis.x * scalar,
.y = vector.axis.y * scalar,
.z = vector.axis.z * scalar,
}};
return result;
}
/**
* @brief Calculates the Hadamard product (element-wise multiplication).
* @param vectorA Vector A.
* @param vectorB Vector B.
* @return Hadamard product.
*/
static inline FusionVector FusionVectorHadamardProduct(const FusionVector vectorA, const FusionVector vectorB)
{
const FusionVector result = {.axis = {
.x = vectorA.axis.x * vectorB.axis.x,
.y = vectorA.axis.y * vectorB.axis.y,
.z = vectorA.axis.z * vectorB.axis.z,
}};
return result;
}
/**
* @brief Returns the cross product.
* @param vectorA Vector A.
* @param vectorB Vector B.
* @return Cross product.
*/
static inline FusionVector FusionVectorCrossProduct(const FusionVector vectorA, const FusionVector vectorB)
{
#define A vectorA.axis
#define B vectorB.axis
const FusionVector result = {.axis = {
.x = A.y * B.z - A.z * B.y,
.y = A.z * B.x - A.x * B.z,
.z = A.x * B.y - A.y * B.x,
}};
return result;
#undef A
#undef B
}
/**
* @brief Returns the dot product.
* @param vectorA Vector A.
* @param vectorB Vector B.
* @return Dot product.
*/
static inline float FusionVectorDotProduct(const FusionVector vectorA, const FusionVector vectorB)
{
return FusionVectorSum(FusionVectorHadamardProduct(vectorA, vectorB));
}
/**
* @brief Returns the vector magnitude squared.
* @param vector Vector.
* @return Vector magnitude squared.
*/
static inline float FusionVectorMagnitudeSquared(const FusionVector vector)
{
return FusionVectorSum(FusionVectorHadamardProduct(vector, vector));
}
/**
* @brief Returns the vector magnitude.
* @param vector Vector.
* @return Vector magnitude.
*/
static inline float FusionVectorMagnitude(const FusionVector vector)
{
return sqrtf(FusionVectorMagnitudeSquared(vector));
}
/**
* @brief Returns the normalised vector.
* @param vector Vector.
* @return Normalised vector.
*/
static inline FusionVector FusionVectorNormalise(const FusionVector vector)
{
#ifdef FUSION_USE_NORMAL_SQRT
const float magnitudeReciprocal = 1.0f / sqrtf(FusionVectorMagnitudeSquared(vector));
#else
const float magnitudeReciprocal = FusionFastInverseSqrt(FusionVectorMagnitudeSquared(vector));
#endif
return FusionVectorMultiplyScalar(vector, magnitudeReciprocal);
}
//------------------------------------------------------------------------------
// Inline functions - Quaternion operations
/**
* @brief Returns the sum of two quaternions.
* @param quaternionA Quaternion A.
* @param quaternionB Quaternion B.
* @return Sum of two quaternions.
*/
static inline FusionQuaternion FusionQuaternionAdd(const FusionQuaternion quaternionA, const FusionQuaternion quaternionB)
{
const FusionQuaternion result = {.element = {
.w = quaternionA.element.w + quaternionB.element.w,
.x = quaternionA.element.x + quaternionB.element.x,
.y = quaternionA.element.y + quaternionB.element.y,
.z = quaternionA.element.z + quaternionB.element.z,
}};
return result;
}
/**
* @brief Returns the multiplication of two quaternions.
* @param quaternionA Quaternion A (to be post-multiplied).
* @param quaternionB Quaternion B (to be pre-multiplied).
* @return Multiplication of two quaternions.
*/
static inline FusionQuaternion FusionQuaternionMultiply(const FusionQuaternion quaternionA, const FusionQuaternion quaternionB)
{
#define A quaternionA.element
#define B quaternionB.element
const FusionQuaternion result = {.element = {
.w = A.w * B.w - A.x * B.x - A.y * B.y - A.z * B.z,
.x = A.w * B.x + A.x * B.w + A.y * B.z - A.z * B.y,
.y = A.w * B.y - A.x * B.z + A.y * B.w + A.z * B.x,
.z = A.w * B.z + A.x * B.y - A.y * B.x + A.z * B.w,
}};
return result;
#undef A
#undef B
}
/**
* @brief Returns the multiplication of a quaternion with a vector. This is a
* normal quaternion multiplication where the vector is treated a
* quaternion with a W element value of zero. The quaternion is post-
* multiplied by the vector.
* @param quaternion Quaternion.
* @param vector Vector.
* @return Multiplication of a quaternion with a vector.
*/
static inline FusionQuaternion FusionQuaternionMultiplyVector(const FusionQuaternion quaternion, const FusionVector vector)
{
#define Q quaternion.element
#define V vector.axis
const FusionQuaternion result = {.element = {
.w = -Q.x * V.x - Q.y * V.y - Q.z * V.z,
.x = Q.w * V.x + Q.y * V.z - Q.z * V.y,
.y = Q.w * V.y - Q.x * V.z + Q.z * V.x,
.z = Q.w * V.z + Q.x * V.y - Q.y * V.x,
}};
return result;
#undef Q
#undef V
}
/**
* @brief Returns the normalised quaternion.
* @param quaternion Quaternion.
* @return Normalised quaternion.
*/
static inline FusionQuaternion FusionQuaternionNormalise(const FusionQuaternion quaternion)
{
#define Q quaternion.element
#ifdef FUSION_USE_NORMAL_SQRT
const float magnitudeReciprocal = 1.0f / sqrtf(Q.w * Q.w + Q.x * Q.x + Q.y * Q.y + Q.z * Q.z);
#else
const float magnitudeReciprocal = FusionFastInverseSqrt(Q.w * Q.w + Q.x * Q.x + Q.y * Q.y + Q.z * Q.z);
#endif
const FusionQuaternion result = {.element = {
.w = Q.w * magnitudeReciprocal,
.x = Q.x * magnitudeReciprocal,
.y = Q.y * magnitudeReciprocal,
.z = Q.z * magnitudeReciprocal,
}};
return result;
#undef Q
}
//------------------------------------------------------------------------------
// Inline functions - Matrix operations
/**
* @brief Returns the multiplication of a matrix with a vector.
* @param matrix Matrix.
* @param vector Vector.
* @return Multiplication of a matrix with a vector.
*/
static inline FusionVector FusionMatrixMultiplyVector(const FusionMatrix matrix, const FusionVector vector)
{
#define R matrix.element
const FusionVector result = {.axis = {
.x = R.xx * vector.axis.x + R.xy * vector.axis.y + R.xz * vector.axis.z,
.y = R.yx * vector.axis.x + R.yy * vector.axis.y + R.yz * vector.axis.z,
.z = R.zx * vector.axis.x + R.zy * vector.axis.y + R.zz * vector.axis.z,
}};
return result;
#undef R
}
//------------------------------------------------------------------------------
// Inline functions - Conversion operations
/**
* @brief Converts a quaternion to a rotation matrix.
* @param quaternion Quaternion.
* @return Rotation matrix.
*/
static inline FusionMatrix FusionQuaternionToMatrix(const FusionQuaternion quaternion)
{
#define Q quaternion.element
const float qwqw = Q.w * Q.w; // calculate common terms to avoid repeated operations
const float qwqx = Q.w * Q.x;
const float qwqy = Q.w * Q.y;
const float qwqz = Q.w * Q.z;
const float qxqy = Q.x * Q.y;
const float qxqz = Q.x * Q.z;
const float qyqz = Q.y * Q.z;
const FusionMatrix matrix = {.element = {
.xx = 2.0f * (qwqw - 0.5f + Q.x * Q.x),
.xy = 2.0f * (qxqy - qwqz),
.xz = 2.0f * (qxqz + qwqy),
.yx = 2.0f * (qxqy + qwqz),
.yy = 2.0f * (qwqw - 0.5f + Q.y * Q.y),
.yz = 2.0f * (qyqz - qwqx),
.zx = 2.0f * (qxqz - qwqy),
.zy = 2.0f * (qyqz + qwqx),
.zz = 2.0f * (qwqw - 0.5f + Q.z * Q.z),
}};
return matrix;
#undef Q
}
/**
* @brief Converts a quaternion to ZYX Euler angles in degrees.
* @param quaternion Quaternion.
* @return Euler angles in degrees.
*/
static inline FusionEuler FusionQuaternionToEuler(const FusionQuaternion quaternion)
{
#define Q quaternion.element
const float halfMinusQySquared = 0.5f - Q.y * Q.y; // calculate common terms to avoid repeated operations
const FusionEuler euler = {.angle = {
.roll = FusionRadiansToDegrees(atan2f(Q.w * Q.x + Q.y * Q.z, halfMinusQySquared - Q.x * Q.x)),
.pitch = FusionRadiansToDegrees(FusionAsin(2.0f * (Q.w * Q.y - Q.z * Q.x))),
.yaw = FusionRadiansToDegrees(atan2f(Q.w * Q.z + Q.x * Q.y, halfMinusQySquared - Q.z * Q.z)),
}};
return euler;
#undef Q
}
#endif
//------------------------------------------------------------------------------
// End of file

80
src/Fusion/FusionOffset.c 100644
Wyświetl plik

@ -0,0 +1,80 @@
/**
* @file FusionOffset.c
* @author Seb Madgwick
* @brief Gyroscope offset correction algorithm for run-time calibration of the
* gyroscope offset.
*/
//------------------------------------------------------------------------------
// Includes
#include "FusionOffset.h"
#include <math.h> // fabsf
//------------------------------------------------------------------------------
// Definitions
/**
* @brief Cutoff frequency in Hz.
*/
#define CUTOFF_FREQUENCY (0.02f)
/**
* @brief Timeout in seconds.
*/
#define TIMEOUT (5)
/**
* @brief Threshold in degrees per second.
*/
#define THRESHOLD (3.0f)
//------------------------------------------------------------------------------
// Functions
/**
* @brief Initialises the gyroscope offset algorithm.
* @param offset Gyroscope offset algorithm structure.
* @param sampleRate Sample rate in Hz.
*/
void FusionOffsetInitialise(FusionOffset *const offset, const unsigned int sampleRate)
{
offset->filterCoefficient = 2.0f * (float)M_PI * CUTOFF_FREQUENCY * (1.0f / (float)sampleRate);
offset->timeout = TIMEOUT * sampleRate;
offset->timer = 0;
offset->gyroscopeOffset = FUSION_VECTOR_ZERO;
}
/**
* @brief Updates the gyroscope offset algorithm and returns the corrected
* gyroscope measurement.
* @param offset Gyroscope offset algorithm structure.
* @param gyroscope Gyroscope measurement in degrees per second.
* @return Corrected gyroscope measurement in degrees per second.
*/
FusionVector FusionOffsetUpdate(FusionOffset *const offset, FusionVector gyroscope)
{
// Subtract offset from gyroscope measurement
gyroscope = FusionVectorSubtract(gyroscope, offset->gyroscopeOffset);
// Reset timer if gyroscope not stationary
if ((fabsf(gyroscope.axis.x) > THRESHOLD) || (fabsf(gyroscope.axis.y) > THRESHOLD) || (fabsf(gyroscope.axis.z) > THRESHOLD)) {
offset->timer = 0;
return gyroscope;
}
// Increment timer while gyroscope stationary
if (offset->timer < offset->timeout) {
offset->timer++;
return gyroscope;
}
// Adjust offset if timer has elapsed
offset->gyroscopeOffset =
FusionVectorAdd(offset->gyroscopeOffset, FusionVectorMultiplyScalar(gyroscope, offset->filterCoefficient));
return gyroscope;
}
//------------------------------------------------------------------------------
// End of file

40
src/Fusion/FusionOffset.h 100644
Wyświetl plik

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

1
src/configuration.h
Wyświetl plik

@ -144,6 +144,7 @@ along with this program. If not, see <http://www.gnu.org/licenses/>.
#define LIS3DH_ADR 0x18
#define BMA423_ADDR 0x19
#define LSM6DS3_ADDR 0x6A
#define BMX160_ADDR 0x69
// -----------------------------------------------------------------------------
// LED

4
src/detect/ScanI2C.cpp
Wyświetl plik

@ -36,8 +36,8 @@ ScanI2C::FoundDevice ScanI2C::firstKeyboard() const
ScanI2C::FoundDevice ScanI2C::firstAccelerometer() const
{
ScanI2C::DeviceType types[] = {MPU6050, LIS3DH, BMA423, LSM6DS3};
return firstOfOrNONE(4, types);
ScanI2C::DeviceType types[] = {MPU6050, LIS3DH, BMA423, LSM6DS3, BMX160};
return firstOfOrNONE(5, types);
}
ScanI2C::FoundDevice ScanI2C::find(ScanI2C::DeviceType) const

3
src/detect/ScanI2C.h
Wyświetl plik

@ -49,7 +49,8 @@ class ScanI2C
OPT3001,
MLX90632,
AHT10,
DFROBOT_LARK,
BMX160,
DFROBOT_LARK
} DeviceType;
// typedef uint8_t DeviceAddress;

1
src/detect/ScanI2CTwoWire.cpp
Wyświetl plik

@ -342,6 +342,7 @@ void ScanI2CTwoWire::scanPort(I2CPort port)
SCAN_SIMPLE_CASE(PMSA0031_ADDR, PMSA0031, "PMSA0031 air quality sensor found\n")
SCAN_SIMPLE_CASE(MPU6050_ADDR, MPU6050, "MPU6050 accelerometer found\n");
SCAN_SIMPLE_CASE(BMX160_ADDR, BMX160, "BMX160 accelerometer found\n");
SCAN_SIMPLE_CASE(BMA423_ADDR, BMA423, "BMA423 accelerometer found\n");
SCAN_SIMPLE_CASE(LSM6DS3_ADDR, LSM6DS3, "LSM6DS3 accelerometer found at address 0x%x\n", (uint8_t)addr.address);
SCAN_SIMPLE_CASE(TCA9555_ADDR, TCA9555, "TCA9555 I2C expander found\n");

8
src/graphics/Screen.cpp
Wyświetl plik

@ -1516,9 +1516,13 @@ static void drawNodeInfo(OLEDDisplay *display, OLEDDisplayUiState *state, int16_
}
bool hasNodeHeading = false;
if (ourNode && hasValidPosition(ourNode)) {
if (ourNode && (hasValidPosition(ourNode) || screen->hasHeading())) {
const meshtastic_PositionLite &op = ourNode->position;
float myHeading = estimatedHeading(DegD(op.latitude_i), DegD(op.longitude_i));
float myHeading;
if (screen->hasHeading())
myHeading = (screen->getHeading()) * PI / 180; // gotta convert compass degrees to Radians
else
myHeading = estimatedHeading(DegD(op.latitude_i), DegD(op.longitude_i));
drawCompassNorth(display, compassX, compassY, myHeading);
if (hasValidPosition(node)) {

13
src/graphics/Screen.h
Wyświetl plik

@ -204,6 +204,17 @@ class Screen : public concurrency::OSThread
enqueueCmd(cmd);
}
// Function to allow the AccelerometerThread to set the heading if a sensor provides it
// Mutex needed?
void setHeading(long _heading)
{
hasCompass = true;
compassHeading = _heading;
}
bool hasHeading() { return hasCompass; }
long getHeading() { return compassHeading; }
// functions for display brightness
void increaseBrightness();
void decreaseBrightness();
@ -428,6 +439,8 @@ class Screen : public concurrency::OSThread
// Implementation to Adjust Brightness
uint8_t brightness = BRIGHTNESS_DEFAULT; // H = 254, MH = 192, ML = 130 L = 103
bool hasCompass = false;
float compassHeading;
/// Holds state for debug information
DebugInfo debugInfo;

6
src/main.h
Wyświetl plik

@ -53,6 +53,9 @@ extern Adafruit_DRV2605 drv;
extern AudioThread *audioThread;
#endif
// Global Screen singleton.
extern graphics::Screen *screen;
#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_ENVIRONMENTAL_SENSOR
#include "AccelerometerThread.h"
extern AccelerometerThread *accelerometerThread;
@ -62,9 +65,6 @@ extern bool isVibrating;
extern int TCPPort; // set by Portduino
// Global Screen singleton.
extern graphics::Screen *screen;
// extern Observable<meshtastic::PowerStatus> newPowerStatus; //TODO: move this to main-esp32.cpp somehow or a helper class
// extern meshtastic::PowerStatus *powerStatus;

1
variants/rak4631/platformio.ini
Wyświetl plik

@ -16,6 +16,7 @@ lib_deps =
melopero/Melopero RV3028@^1.1.0
https://github.com/RAKWireless/RAK13800-W5100S.git#1.0.2
rakwireless/RAKwireless NCP5623 RGB LED library@^1.0.2
beegee-tokyo/RAKwireless RAK12034@^1.0.0
debug_tool = jlink
; If not set we will default to uploading over serial (first it forces bootloader entry by talking 1200bps to cdcacm)
;upload_protocol = jlink

1
variants/rak4631_epaper/platformio.ini
Wyświetl plik

@ -13,6 +13,7 @@ lib_deps =
zinggjm/GxEPD2@^1.4.9
melopero/Melopero RV3028@^1.1.0
rakwireless/RAKwireless NCP5623 RGB LED library@^1.0.2
beegee-tokyo/RAKwireless RAK12034@^1.0.0
debug_tool = jlink
; If not set we will default to uploading over serial (first it forces bootloader entry by talking 1200bps to cdcacm)
;upload_protocol = jlink

3
variants/rak4631_epaper_onrxtx/platformio.ini
Wyświetl plik

@ -15,7 +15,8 @@ lib_deps =
zinggjm/GxEPD2@^1.5.1
melopero/Melopero RV3028@^1.1.0
rakwireless/RAKwireless NCP5623 RGB LED library@^1.0.2
beegee-tokyo/RAKwireless RAK12034@^1.0.0
debug_tool = jlink
; If not set we will default to uploading over serial (first it forces bootloader entry by talking 1200bps to cdcacm)
;upload_protocol = jlink
;upload_port = /dev/ttyACM3
;upload_port = /dev/ttyACM3