IMU.cpp

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  
00035 /**
00036  * Includes
00037  */
00038 #include "IMU.h"
00039  
00040 IMU::IMU(float imuRate,
00041          double gyroscopeMeasurementError,
00042          float accelerometerRate,
00043          float gyroscopeRate) : accelerometer(p9, p10), 
00044          gyroscope(p9, p10), imuFilter(imuRate, gyroscopeMeasurementError) {
00045  
00046     imuRate_ = imuRate;
00047     accelerometerRate_ = accelerometerRate;
00048     gyroscopeRate_ = gyroscopeRate;
00049  
00050     a_xAccumulator = 0;
00051     a_yAccumulator = 0;
00052     a_zAccumulator = 0;
00053     w_xAccumulator = 0;
00054     w_yAccumulator = 0;
00055     w_zAccumulator = 0;
00056  
00057     accelerometerSamples = 0;
00058     gyroscopeSamples = 0;
00059  
00060     initializeAccelerometer();
00061     calibrateAccelerometer();
00062  
00063     initializeGyroscope();
00064     calibrateGyroscope();
00065  
00066     accelerometerTicker.attach(this, &IMU::sampleAccelerometer, accelerometerRate_);
00067     gyroscopeTicker.attach(this, &IMU::sampleGyroscope, gyroscopeRate_);
00068     filterTicker.attach(this, &IMU::filter, imuRate_);
00069  
00070 }
00071  
00072 double IMU::getRoll(void) {
00073  
00074     return toDegrees(imuFilter.getRoll());
00075  
00076 }
00077  
00078 double IMU::getPitch(void) {
00079  
00080     return toDegrees(imuFilter.getPitch());
00081  
00082 }
00083  
00084 double IMU::getYaw(void) {
00085  
00086     return toDegrees(imuFilter.getYaw());
00087  
00088 }
00089  
00090 void IMU::initializeAccelerometer(void) {
00091  
00092     //Go into standby mode to configure the device.
00093     accelerometer.setPowerControl(0x00);
00094     //Full resolution, +/-16g, 4mg/LSB.
00095     accelerometer.setDataFormatControl(0x0B);
00096     //200Hz data rate.
00097     accelerometer.setDataRate(ADXL345_200HZ);
00098     //Measurement mode.
00099     accelerometer.setPowerControl(0x08);
00100     //See http://www.analog.com/static/imported-files/application_notes/AN-1077.pdf
00101     wait_ms(22);
00102  
00103 }
00104  
00105 void IMU::sampleAccelerometer(void) {
00106  
00107     if (accelerometerSamples == SAMPLES) {
00108  
00109         a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN;
00110         a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN;
00111         a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN;
00112  
00113         a_xAccumulator = 0;
00114         a_yAccumulator = 0;
00115         a_zAccumulator = 0;
00116         accelerometerSamples = 0;
00117  
00118     } else {
00119  
00120         accelerometer.getOutput(readings);
00121  
00122         a_xAccumulator += (int16_t) readings[0];
00123         a_yAccumulator += (int16_t) readings[1];
00124         a_zAccumulator += (int16_t) readings[2];
00125  
00126         accelerometerSamples++;
00127  
00128     }
00129  
00130 }
00131  
00132 void IMU::calibrateAccelerometer(void) {
00133  
00134     a_xAccumulator = 0;
00135     a_yAccumulator = 0;
00136     a_zAccumulator = 0;
00137  
00138     for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
00139  
00140         accelerometer.getOutput(readings);
00141  
00142         a_xAccumulator += (int16_t) readings[0];
00143         a_yAccumulator += (int16_t) readings[1];
00144         a_zAccumulator += (int16_t) readings[2];
00145  
00146         wait(accelerometerRate_);
00147  
00148     }
00149  
00150     a_xAccumulator /= CALIBRATION_SAMPLES;
00151     a_yAccumulator /= CALIBRATION_SAMPLES;
00152     a_zAccumulator /= CALIBRATION_SAMPLES;
00153  
00154     a_xBias = a_xAccumulator;
00155     a_yBias = a_yAccumulator;
00156     a_zBias = (a_zAccumulator - 250);
00157  
00158     a_xAccumulator = 0;
00159     a_yAccumulator = 0;
00160     a_zAccumulator = 0;
00161  
00162 }
00163  
00164 void IMU::initializeGyroscope(void) {
00165  
00166     //Low pass filter bandwidth of 42Hz.
00167     gyroscope.setLpBandwidth(LPFBW_42HZ);
00168     //Internal sample rate of 200Hz.
00169     gyroscope.setSampleRateDivider(4);
00170  
00171 }
00172  
00173 void IMU::calibrateGyroscope(void) {
00174  
00175     w_xAccumulator = 0;
00176     w_yAccumulator = 0;
00177     w_zAccumulator = 0;
00178  
00179     for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
00180  
00181         w_xAccumulator += gyroscope.getGyroX();
00182         w_yAccumulator += gyroscope.getGyroY();
00183         w_zAccumulator += gyroscope.getGyroZ();
00184         wait(gyroscopeRate_);
00185  
00186     }
00187  
00188     //Average the samples.
00189     w_xAccumulator /= CALIBRATION_SAMPLES;
00190     w_yAccumulator /= CALIBRATION_SAMPLES;
00191     w_zAccumulator /= CALIBRATION_SAMPLES;
00192  
00193     w_xBias = w_xAccumulator;
00194     w_yBias = w_yAccumulator;
00195     w_zBias = w_zAccumulator;
00196  
00197     w_xAccumulator = 0;
00198     w_yAccumulator = 0;
00199     w_zAccumulator = 0;
00200  
00201 }
00202  
00203 void IMU::sampleGyroscope(void) {
00204  
00205     if (gyroscopeSamples == SAMPLES) {
00206  
00207         w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN);
00208         w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN);
00209         w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN);
00210  
00211         w_xAccumulator = 0;
00212         w_yAccumulator = 0;
00213         w_zAccumulator = 0;
00214         gyroscopeSamples = 0;
00215  
00216     } else {
00217  
00218         w_xAccumulator += gyroscope.getGyroX();
00219         w_yAccumulator += gyroscope.getGyroY();
00220         w_zAccumulator += gyroscope.getGyroZ();
00221  
00222         gyroscopeSamples++;
00223  
00224     }
00225  
00226 }
00227  
00228 void IMU::filter(void) {
00229  
00230     //Update the filter variables.
00231     imuFilter.updateFilter(w_y, w_x, w_z, a_y, a_x, a_z);
00232     //Calculate the new Euler angles.
00233     imuFilter.computeEuler();
00234  
00235 }