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 class IMU_Filter
00032 {
00033     public:
00034         IMU_Filter();
00035         void compute(float dt, const float * gyro_data, const float * acc_data, const float * Comp_data);
00036         float angle[3];                                 // calculated values of the position [0: x,roll | 1: y,pitch | 2: z,yaw]
00037         
00038         // IMU/AHRS
00039         float q0, q1, q2, q3;   // quaternion elements representing the estimated orientation
00040         float exInt , eyInt , ezInt;  // scaled integral error
00041         void IMUupdate(float halfT, float gx, float gy, float gz, float ax, float ay, float az);                                // 6DOF without compass
00042         void AHRSupdate(float halfT, float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); // 9DOF with compass
00043     private:
00044         float d_Gyro_angle[3];
00045         void get_Acc_angle(const float * Acc_data);
00046         float Acc_angle[3];
00047 };
00048 
00049 #endif