An fully working IMU-Filter and Sensor drivers for the 10DOF-Board over I2C. All in one simple class. Include, calibrate sensors, call read, get angles. (3D Visualisation code for Python also included) Sensors: L3G4200D, ADXL345, HMC5883, BMP085
IMU/IMU_Filter/IMU_Filter.h@4:f62337b907e5, 2013-08-29 (annotated)
- Committer:
- maetugr
- Date:
- Thu Aug 29 13:52:30 2013 +0000
- Revision:
- 4:f62337b907e5
- Parent:
- 0:3e7450f1a938
The Altitude Sensor is now implemented, it's really 10DOF now ;); TODO: Autocalibration
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
maetugr | 4:f62337b907e5 | 1 | //===================================================================================================== |
maetugr | 4:f62337b907e5 | 2 | // ALGORITHM COPIED FROM http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/ |
maetugr | 4:f62337b907e5 | 3 | // Algorithm Author: S.O.H. Madgwick |
maetugr | 4:f62337b907e5 | 4 | // Algorithm Date: 25th August 2010 |
maetugr | 4:f62337b907e5 | 5 | //===================================================================================================== |
maetugr | 4:f62337b907e5 | 6 | // Description: |
maetugr | 4:f62337b907e5 | 7 | // |
maetugr | 4:f62337b907e5 | 8 | // Quaternion implementation of the 'DCM filter' [Mayhony et al]. Incorporates the magnetic distortion |
maetugr | 4:f62337b907e5 | 9 | // compensation algorithms from my filter [Madgwick] which eliminates the need for a reference |
maetugr | 4:f62337b907e5 | 10 | // direction of flux (bx bz) to be predefined and limits the effect of magnetic distortions to yaw |
maetugr | 4:f62337b907e5 | 11 | // axis only. |
maetugr | 4:f62337b907e5 | 12 | // |
maetugr | 4:f62337b907e5 | 13 | // User must define 'halfT' as the (sample period / 2), and the filter gains 'Kp' and 'Ki'. |
maetugr | 4:f62337b907e5 | 14 | // |
maetugr | 4:f62337b907e5 | 15 | // Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated |
maetugr | 4:f62337b907e5 | 16 | // orientation. See my report for an overview of the use of quaternions in this application. |
maetugr | 4:f62337b907e5 | 17 | // |
maetugr | 4:f62337b907e5 | 18 | // User must call 'AHRSupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz'), |
maetugr | 4:f62337b907e5 | 19 | // accelerometer ('ax', 'ay', 'ay') and magnetometer ('mx', 'my', 'mz') data. Gyroscope units are |
maetugr | 4:f62337b907e5 | 20 | // radians/second, accelerometer and magnetometer units are irrelevant as the vector is normalised. |
maetugr | 4:f62337b907e5 | 21 | // |
maetugr | 4:f62337b907e5 | 22 | //===================================================================================================== |
maetugr | 4:f62337b907e5 | 23 | |
maetugr | 0:3e7450f1a938 | 24 | // by MaEtUgR |
maetugr | 0:3e7450f1a938 | 25 | |
maetugr | 0:3e7450f1a938 | 26 | #ifndef IMU_FILTER_H |
maetugr | 0:3e7450f1a938 | 27 | #define IMU_FILTER_H |
maetugr | 0:3e7450f1a938 | 28 | |
maetugr | 0:3e7450f1a938 | 29 | #include "mbed.h" |
maetugr | 0:3e7450f1a938 | 30 | |
maetugr | 0:3e7450f1a938 | 31 | #define Rad2Deg 57.295779513082320876798154814105 // factor between radians and degrees of angle (180/Pi) |
maetugr | 0:3e7450f1a938 | 32 | |
maetugr | 0:3e7450f1a938 | 33 | class IMU_Filter |
maetugr | 0:3e7450f1a938 | 34 | { |
maetugr | 0:3e7450f1a938 | 35 | public: |
maetugr | 0:3e7450f1a938 | 36 | IMU_Filter(); |
maetugr | 0:3e7450f1a938 | 37 | void compute(float dt, const float * gyro_data, const float * acc_data, const float * Comp_data); |
maetugr | 0:3e7450f1a938 | 38 | float angle[3]; // calculated values of the position [0: x,roll | 1: y,pitch | 2: z,yaw] |
maetugr | 0:3e7450f1a938 | 39 | |
maetugr | 0:3e7450f1a938 | 40 | // IMU/AHRS |
maetugr | 0:3e7450f1a938 | 41 | float q0, q1, q2, q3; // quaternion elements representing the estimated orientation |
maetugr | 0:3e7450f1a938 | 42 | float exInt , eyInt , ezInt; // scaled integral error |
maetugr | 0:3e7450f1a938 | 43 | void IMUupdate(float halfT, float gx, float gy, float gz, float ax, float ay, float az); |
maetugr | 0:3e7450f1a938 | 44 | void AHRSupdate(float halfT, float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); |
maetugr | 0:3e7450f1a938 | 45 | private: |
maetugr | 0:3e7450f1a938 | 46 | float d_Gyro_angle[3]; |
maetugr | 0:3e7450f1a938 | 47 | void get_Acc_angle(const float * Acc_data); |
maetugr | 0:3e7450f1a938 | 48 | float Acc_angle[3]; |
maetugr | 0:3e7450f1a938 | 49 | }; |
maetugr | 0:3e7450f1a938 | 50 | |
maetugr | 0:3e7450f1a938 | 51 | #endif |