mirror of https://github.com/rusefi/bldc.git
84 lines
3.9 KiB
C
84 lines
3.9 KiB
C
/**
|
|
* @file FusionAhrs.h
|
|
* @author Seb Madgwick
|
|
* @brief The AHRS sensor fusion algorithm to combines gyroscope, accelerometer,
|
|
* and magnetometer measurements into a single measurement of orientation
|
|
* relative to the Earth (NWU convention).
|
|
*
|
|
* The algorithm behaviour is governed by a gain. A low gain will decrease the
|
|
* influence of the accelerometer and magnetometer so that the algorithm will
|
|
* better reject disturbances causes by translational motion and temporary
|
|
* magnetic distortions. However, a low gain will also increase the risk of
|
|
* drift due to gyroscope calibration errors. A typical gain value suitable for
|
|
* most applications is 0.5.
|
|
*
|
|
* The algorithm allows the application to define a minimum and maximum valid
|
|
* magnetic field magnitude. The algorithm will ignore magnetic measurements
|
|
* that fall outside of this range. This allows the algorithm to reject
|
|
* magnetic measurements that do not represent the direction of magnetic North.
|
|
* The typical magnitude of the Earth's magnetic field is between 20 uT and
|
|
* 70 uT.
|
|
*
|
|
* The algorithm can be used without a magnetometer. Measurements of
|
|
* orientation obtained using only gyroscope and accelerometer measurements
|
|
* can be expected to drift in the yaw component of orientation only. The
|
|
* application can reset the drift in yaw by setting the yaw to a specified
|
|
* angle at any time.
|
|
*
|
|
* The algorithm provides the measurement of orientation as a quaternion. The
|
|
* library includes functions for converting this quaternion to a rotation
|
|
* matrix and Euler angles.
|
|
*
|
|
* The algorithm also provides a measurement of linear acceleration and Earth
|
|
* acceleration. Linear acceleration is equal to the accelerometer measurement
|
|
* with the 1 g of gravity removed. Earth acceleration is a measurement of
|
|
* linear acceleration in the Earth coordinate frame.
|
|
*/
|
|
|
|
#ifndef FUSION_AHRS_H
|
|
#define FUSION_AHRS_H
|
|
|
|
//------------------------------------------------------------------------------
|
|
// Includes
|
|
|
|
#include "FusionTypes.h"
|
|
#include <stdbool.h>
|
|
|
|
//------------------------------------------------------------------------------
|
|
// Definitions
|
|
|
|
/**
|
|
* @brief AHRS algorithm structure. Structure members are used internally and
|
|
* should not be used by the user application.
|
|
*/
|
|
typedef struct {
|
|
float gain;
|
|
float acc_conf_decay;
|
|
float minimumMagneticFieldSquared;
|
|
float maximumMagneticFieldSquared;
|
|
FusionQuaternion quaternion; // describes the Earth relative to the sensor
|
|
FusionVector3 linearAcceleration;
|
|
float accMagP;
|
|
} FusionAhrs;
|
|
|
|
//------------------------------------------------------------------------------
|
|
// Function prototypes
|
|
|
|
void FusionAhrsInitialise(FusionAhrs * const fusionAhrs, const float gain, const float acc_conf_decay);
|
|
void FusionAhrsSetGain(FusionAhrs * const fusionAhrs, const float gain);
|
|
void FusionAhrsSetAccConfDecay(FusionAhrs * const fusionAhrs, const float acc_conf_decay);
|
|
void FusionAhrsSetMagneticField(FusionAhrs * const fusionAhrs, const float minimumMagneticField, const float maximumMagneticField);
|
|
void FusionAhrsUpdate(FusionAhrs * const fusionAhrs, const FusionVector3 gyroscope, const FusionVector3 accelerometer, const FusionVector3 magnetometer, const float samplePeriod);
|
|
void FusionAhrsUpdateWithoutMagnetometer(FusionAhrs * const fusionAhrs, const FusionVector3 gyroscope, const FusionVector3 accelerometer, const float samplePeriod);
|
|
FusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs * const fusionAhrs);
|
|
FusionVector3 FusionAhrsGetLinearAcceleration(const FusionAhrs * const fusionAhrs);
|
|
FusionVector3 FusionAhrsGetEarthAcceleration(const FusionAhrs * const fusionAhrs);
|
|
void FusionAhrsReinitialise(FusionAhrs * const fusionAhrs);
|
|
bool FusionAhrsIsInitialising(const FusionAhrs * const fusionAhrs);
|
|
void FusionAhrsSetYaw(FusionAhrs * const fusionAhrs, const float yaw);
|
|
|
|
#endif
|
|
|
|
//------------------------------------------------------------------------------
|
|
// End of file
|