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.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     /**
00113      * Sample the sensors, and if enough samples have been taken,
00114      * take an average, and compute the new Euler angles.
00115      */
00116     void sample(void);
00117     
00118     /**
00119      * Recalibrate the sensors and reset the IMU filter.
00120      */
00121     void reset(void);
00122 
00123 private:
00124 
00125     /**
00126      * Set up the ADXL345 appropriately.
00127      */
00128     void initializeAccelerometer(void);
00129 
00130     /**
00131      * Calculate the zero g offset.
00132      */
00133     void calibrateAccelerometer(void);
00134 
00135     /**
00136      * Take a set of samples and average them.
00137      */
00138     void sampleAccelerometer(void);
00139 
00140     /**
00141      * Set up the ITG-3200 appropriately.
00142      */
00143     void initializeGyroscope(void);
00144 
00145     /**
00146      * Calculate the bias offset.
00147      */
00148     void calibrateGyroscope(void);
00149 
00150     /**
00151      * Take a set of samples and average them.
00152      */
00153     void sampleGyroscope(void);
00154 
00155     /**
00156      * Update the filter and calculate the Euler angles.
00157      */
00158     void filter(void);
00159 
00160     ADXL345   accelerometer;
00161     ITG3200   gyroscope;
00162     IMUfilter imuFilter;
00163 
00164     Ticker accelerometerTicker;
00165     Ticker gyroscopeTicker;
00166     Ticker sampleTicker;
00167     Ticker filterTicker;
00168 
00169     float accelerometerRate_;
00170     float gyroscopeRate_;
00171     float imuRate_;
00172 
00173     //Offsets for the gyroscope.
00174     //The readings we take when the gyroscope is stationary won't be 0, so we'll
00175     //average a set of readings we do get when the gyroscope is stationary and
00176     //take those away from subsequent readings to ensure the gyroscope is offset
00177     //or biased to 0.
00178     double w_xBias;
00179     double w_yBias;
00180     double w_zBias;
00181 
00182     double a_xBias;
00183     double a_yBias;
00184     double a_zBias;
00185 
00186     volatile double a_xAccumulator;
00187     volatile double a_yAccumulator;
00188     volatile double a_zAccumulator;
00189     volatile double w_xAccumulator;
00190     volatile double w_yAccumulator;
00191     volatile double w_zAccumulator;
00192 
00193     //Accelerometer and gyroscope readings for x, y, z axes.
00194     volatile double a_x;
00195     volatile double a_y;
00196     volatile double a_z;
00197     volatile double w_x;
00198     volatile double w_y;
00199     volatile double w_z;
00200 
00201     //Buffer for accelerometer readings.
00202     int readings[3];
00203     int accelerometerSamples;
00204     int gyroscopeSamples;
00205 
00206 };
00207 
00208 #endif /* MBED_IMU_H */