IMU.h

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