IMU_Filter.h

00001 //=====================================================================================================
00002 // ALGORITHM COPIED FROM http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
00003 // Algorithm Author: S.O.H. Madgwick
00004 // Algorithm Date: 25th August 2010
00005 //=====================================================================================================
00006 // Description:
00007 //
00008 // Quaternion implementation of the 'DCM filter' [Mayhony et al].  Incorporates the magnetic distortion
00009 // compensation algorithms from my filter [Madgwick] which eliminates the need for a reference
00010 // direction of flux (bx bz) to be predefined and limits the effect of magnetic distortions to yaw
00011 // axis only.
00012 //
00013 // User must define 'halfT' as the (sample period / 2), and the filter gains 'Kp' and 'Ki'.
00014 //
00015 // Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated
00016 // orientation.  See my report for an overview of the use of quaternions in this application.
00017 //
00018 // User must call 'AHRSupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz'),
00019 // accelerometer ('ax', 'ay', 'ay') and magnetometer ('mx', 'my', 'mz') data.  Gyroscope units are
00020 // radians/second, accelerometer and magnetometer units are irrelevant as the vector is normalised.
00021 //
00022 //=====================================================================================================
00023 
00024 // by MaEtUgR
00025 
00026 #ifndef IMU_FILTER_H
00027 #define IMU_FILTER_H
00028 
00029 #include "mbed.h"
00030 
00031 #define Rad2Deg         57.295779513082320876798154814105 // factor between radians and degrees of angle (180/Pi)
00032 
00033 class IMU_Filter
00034 {
00035     public:
00036         IMU_Filter();
00037         void compute(float dt, const float * gyro_data, const float * acc_data, const float * Comp_data);
00038         float angle[3];                                 // calculated values of the position [0: x,roll | 1: y,pitch | 2: z,yaw]
00039         
00040         // IMU/AHRS
00041         float q0, q1, q2, q3;   // quaternion elements representing the estimated orientation
00042         float exInt , eyInt , ezInt;  // scaled integral error
00043         void IMUupdate(float halfT, float gx, float gy, float gz, float ax, float ay, float az);
00044         void AHRSupdate(float halfT, float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
00045     private:
00046         float d_Gyro_angle[3];
00047         void get_Acc_angle(const float * Acc_data);
00048         float Acc_angle[3];
00049 };
00050 
00051 #endif