Matthias Grob / Mbed 2 deprecated IMU_10DOF

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

Dependencies:   mbed

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?

UserRevisionLine numberNew 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