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(p5, p6, p7, p8), 
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 }