38 lines
1.8 KiB
C
38 lines
1.8 KiB
C
|
//=====================================================================================================
|
||
|
|
// MadgwickAHRS.h
|
||
|
|
//=====================================================================================================
|
||
|
|
//
|
||
|
|
// Implementation of Madgwick's IMU and AHRS algorithms.
|
||
|
|
// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
|
||
|
|
//
|
||
|
|
// Date Author Notes
|
||
|
|
// 29/09/2011 SOH Madgwick Initial release
|
||
|
|
// 02/10/2011 SOH Madgwick Optimised for reduced CPU load
|
||
|
|
//
|
||
|
|
//=====================================================================================================
|
||
|
|
#ifndef MadgwickAHRS_h
|
||
|
|
#define MadgwickAHRS_h
|
||
|
|
|
||
|
|
//----------------------------------------------------------------------------------------------------
|
||
|
|
// Variable declaration
|
||
|
|
|
||
|
|
//extern volatile float beta; // algorithm gain
|
||
|
|
//extern volatile float q0, q1, q2, q3; // quaternion of sensor frame relative to auxiliary frame
|
||
|
|
|
||
|
|
typedef struct {
|
||
|
|
float q[4]; // Quaternion: [qw, qx, qy, qz]
|
||
|
|
float beta; // Filter gain (tunes correction strength)
|
||
|
|
float sample_freq; // Sampling frequency in Hz
|
||
|
|
} MadgwickAHRS_Filter;
|
||
|
|
//---------------------------------------------------------------------------------------------------
|
||
|
|
// Function declarations
|
||
|
|
|
||
|
|
void MadgwickAHRS_init (MadgwickAHRS_Filter* filter, float beta, float sample_freq);
|
||
|
|
void MadgwickAHRS_update(MadgwickAHRS_Filter* filter, float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
|
||
|
|
void MadgwickAHRS_update_IMU(MadgwickAHRS_Filter* filter, float gx, float gy, float gz, float ax, float ay, float az);
|
||
|
|
|
||
|
|
#endif
|
||
|
|
//=====================================================================================================
|
||
|
|
// End of file
|
||
|
|
//=====================================================================================================
|