IMU.h

00001 /**
00002  * @section LICENSE
00003  *
00004  * Copyright (c) 2010 ARM Limited
00005  *
00006  * Permission is hereby granted, free of charge, to any person obtaining a copy
00007  * of this software and associated documentation files (the "Software"), to deal
00008  * in the Software without restriction, including without limitation the rights
00009  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00010  * copies of the Software, and to permit persons to whom the Software is
00011  * furnished to do so, subject to the following conditions:
00012  *
00013  * The above copyright notice and this permission notice shall be included in
00014  * all copies or substantial portions of the Software.
00015  *
00016  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00017  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00018  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00019  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00020  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00021  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00022  * THE SOFTWARE.
00023  *
00024  * @section DESCRIPTION
00025  *
00026  * IMU consisting of ADXL345 accelerometer and ITG-3200 gyroscope using
00027  * orientation filter developed by Sebastian Madgwick.
00028  *
00029  * Find more details about his paper here:
00030  *
00031  * http://code.google.com/p/imumargalgorithm30042010sohm/ 
00032  */
00033  
00034 #ifndef MBED_IMU_H
00035 #define MBED_IMU_H
00036  
00037 /**
00038  * Includes
00039  */
00040 #include "mbed.h"
00041 #include "ADXL345_I2C.h"
00042 #include "ITG3200.h"
00043 #include "IMUfilter.h"
00044  
00045 /**
00046  * Defines
00047  */
00048 #define IMU_RATE           0.025
00049 #define ACCELEROMETER_RATE 0.005
00050 #define GYROSCOPE_RATE     0.005
00051 #define GYRO_MEAS_ERROR    0.3 //IMUfilter tuning parameter.
00052  
00053 //Gravity at Earth's surface in m/s/s
00054 #define g0 9.812865328
00055 //Number of samples to average
00056 #define SAMPLES 4
00057 #define CALIBRATION_SAMPLES 128
00058 //Multiply radians to get degrees.
00059 #define toDegrees(x) (x * 57.2957795)
00060 //Multiply degrees to get radians.
00061 #define toRadians(x) (x * 0.01745329252)
00062 //Full scale resolution on the ADXL345 is 4mg/LSB.
00063 //Multiply ADC count readings from ADXL345 to get acceleration in m/s/s.
00064 #define toAcceleration(x) (x * (4 * g0 * 0.001))
00065 //14.375 LSB/(degrees/sec)
00066 #define GYROSCOPE_GAIN (1 / 14.375)
00067 #define ACCELEROMETER_GAIN (0.004 * g0)
00068  
00069 /**
00070  * IMU consisting of ADXL345 accelerometer and ITG-3200 gyroscope to calculate
00071  * roll, pitch and yaw angles.
00072  */
00073 class IMU {
00074  
00075 public:
00076  
00077     /**
00078      * Constructor.
00079      *
00080      * @param imuRate Rate which IMUfilter update and Euler angle calculation
00081      *                occurs.
00082      * @param gyroscopeMeasurementError IMUfilter tuning parameter.
00083      * @param accelerometerRate Rate at which accelerometer data is sampled.
00084      * @param gyroscopeRate Rate at which gyroscope data is sampled.
00085      */
00086     IMU(float imuRate,
00087         double gyroscopeMeasurementError,
00088         float accelerometerRate,
00089         float gyroscopeRate);
00090  
00091     /**
00092      * Get the current roll angle.
00093      *
00094      * @return The current roll angle in degrees.
00095      */
00096     double getRoll(void);
00097     
00098     /**
00099      * Get the current pitch angle.
00100      *
00101      * @return The current pitch angle in degrees.
00102      */
00103     double getPitch(void);
00104  
00105     /**
00106      * Get the current yaw angle.
00107      *
00108      * @return The current yaw angle in degrees.
00109      */
00110     double getYaw(void);
00111  
00112 private:
00113  
00114     /**
00115      * Set up the ADXL345 appropriately.
00116      */
00117     void initializeAccelerometer(void);
00118  
00119     /**
00120      * Calculate the zero g offset.
00121      */
00122     void calibrateAccelerometer(void);
00123  
00124     /**
00125      * Take a set of samples and average them.
00126      */
00127     void sampleAccelerometer(void);
00128  
00129     /**
00130      * Set up the ITG-3200 appropriately.
00131      */
00132     void initializeGyroscope(void);
00133  
00134     /**
00135      * Calculate the bias offset.
00136      */
00137     void calibrateGyroscope(void);
00138  
00139     /**
00140      * Take a set of samples and average them.
00141      */
00142     void sampleGyroscope(void);
00143  
00144     /**
00145      * Update the filter and calculate the Euler angles.
00146      */
00147     void filter(void);
00148  
00149     //ADXL345   accelerometer;
00150     ADXL345_I2C   accelerometer;
00151     ITG3200   gyroscope;
00152     IMUfilter imuFilter;
00153  
00154     Ticker accelerometerTicker;
00155     Ticker gyroscopeTicker;
00156     Ticker filterTicker;
00157     
00158     float accelerometerRate_;
00159     float gyroscopeRate_;
00160     float imuRate_;
00161  
00162     //Offsets for the gyroscope.
00163     //The readings we take when the gyroscope is stationary won't be 0, so we'll
00164     //average a set of readings we do get when the gyroscope is stationary and
00165     //take those away from subsequent readings to ensure the gyroscope is offset
00166     //or biased to 0.
00167     double w_xBias;
00168     double w_yBias;
00169     double w_zBias;
00170  
00171     double a_xBias;
00172     double a_yBias;
00173     double a_zBias;
00174  
00175     volatile double a_xAccumulator;
00176     volatile double a_yAccumulator;
00177     volatile double a_zAccumulator;
00178     volatile double w_xAccumulator;
00179     volatile double w_yAccumulator;
00180     volatile double w_zAccumulator;
00181  
00182     //Accelerometer and gyroscope readings for x, y, z axes.
00183     volatile double a_x;
00184     volatile double a_y;
00185     volatile double a_z;
00186     volatile double w_x;
00187     volatile double w_y;
00188     volatile double w_z;
00189  
00190     //Buffer for accelerometer readings.
00191     int readings[3];
00192     int accelerometerSamples;
00193     int gyroscopeSamples;
00194  
00195 };
00196  
00197 #endif /* MBED_IMU_H */