IMU.cpp

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 
00037 /**
00038  * Includes
00039  */
00040 #include "IMU.h"
00041 
00042 IMU::IMU(float imuRate,
00043          double gyroscopeMeasurementError,
00044          float accelerometerRate,
00045          float gyroscopeRate) : accelerometer(p5, p6, p7, p8),
00046         gyroscope(p9, p10), imuFilter(imuRate, gyroscopeMeasurementError) {
00047 
00048     imuRate_ = imuRate;
00049     accelerometerRate_ = accelerometerRate;
00050     gyroscopeRate_ = gyroscopeRate;
00051 
00052     //Initialize sampling variables.
00053     a_xAccumulator = 0;
00054     a_yAccumulator = 0;
00055     a_zAccumulator = 0;
00056     w_xAccumulator = 0;
00057     w_yAccumulator = 0;
00058     w_zAccumulator = 0;
00059 
00060     accelerometerSamples = 0;
00061     gyroscopeSamples = 0;
00062 
00063     //Initialize and calibrate sensors.
00064     initializeAccelerometer();
00065     calibrateAccelerometer();
00066 
00067     initializeGyroscope();
00068     calibrateGyroscope();
00069 
00070     //To reduce the number of interrupts we'll remove the separate tickers for
00071     //the accelerometer, gyro and filter update and combine them all into one.
00072 
00073     //accelerometerTicker.attach(this, &IMU::sampleAccelerometer, accelerometerRate_);
00074     //gyroscopeTicker.attach(this, &IMU::sampleGyroscope, gyroscopeRate_);
00075     sampleTicker.attach(this, &IMU::sample, accelerometerRate_);
00076     //filterTicker.attach(this, &IMU::filter, imuRate_);
00077 
00078 }
00079 
00080 double IMU::getRoll(void) {
00081 
00082     return toDegrees(imuFilter.getRoll());
00083 
00084 }
00085 
00086 double IMU::getPitch(void) {
00087 
00088     return toDegrees(imuFilter.getPitch());
00089 
00090 }
00091 
00092 double IMU::getYaw(void) {
00093 
00094     return toDegrees(imuFilter.getYaw());
00095 
00096 }
00097 
00098 void IMU::initializeAccelerometer(void) {
00099 
00100     //Go into standby mode to configure the device.
00101     accelerometer.setPowerControl(0x00);
00102     //Full resolution, +/-16g, 4mg/LSB.
00103     accelerometer.setDataFormatControl(0x0B);
00104     //200Hz data rate.
00105     accelerometer.setDataRate(ADXL345_200HZ);
00106     //Measurement mode.
00107     accelerometer.setPowerControl(0x08);
00108     //See http://www.analog.com/static/imported-files/application_notes/AN-1077.pdf
00109     wait_ms(22);
00110 
00111 }
00112 
00113 void IMU::sampleAccelerometer(void) {
00114 
00115     //If we've taken a certain number of samples,
00116     //average them, remove the bias and convert the units.
00117     if (accelerometerSamples == SAMPLES) {
00118 
00119         a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN;
00120         a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN;
00121         a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN;
00122 
00123         a_xAccumulator = 0;
00124         a_yAccumulator = 0;
00125         a_zAccumulator = 0;
00126         accelerometerSamples = 0;
00127 
00128     }
00129     //Otherwise, accumulate another reading. 
00130     else {
00131 
00132         accelerometer.getOutput(readings);
00133 
00134         a_xAccumulator += (int16_t) readings[0];
00135         a_yAccumulator += (int16_t) readings[1];
00136         a_zAccumulator += (int16_t) readings[2];
00137 
00138         accelerometerSamples++;
00139 
00140     }
00141 
00142 }
00143 
00144 void IMU::calibrateAccelerometer(void) {
00145 
00146     a_xAccumulator = 0;
00147     a_yAccumulator = 0;
00148     a_zAccumulator = 0;
00149 
00150     //Accumulate a certain number of samples.
00151     for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
00152 
00153         accelerometer.getOutput(readings);
00154 
00155         a_xAccumulator += (int16_t) readings[0];
00156         a_yAccumulator += (int16_t) readings[1];
00157         a_zAccumulator += (int16_t) readings[2];
00158 
00159         wait(accelerometerRate_);
00160 
00161     }
00162 
00163     //Average the samples.
00164     a_xAccumulator /= CALIBRATION_SAMPLES;
00165     a_yAccumulator /= CALIBRATION_SAMPLES;
00166     a_zAccumulator /= CALIBRATION_SAMPLES;
00167 
00168     //These are our zero g offsets.
00169     //250 = 9.81m/s/s @ 4mg/LSB.
00170     a_xBias = a_xAccumulator;
00171     a_yBias = a_yAccumulator;
00172     a_zBias = (a_zAccumulator - 250);
00173 
00174     //Reset accumulators.
00175     a_xAccumulator = 0;
00176     a_yAccumulator = 0;
00177     a_zAccumulator = 0;
00178 
00179 }
00180 
00181 void IMU::initializeGyroscope(void) {
00182 
00183     //Low pass filter bandwidth of 42Hz.
00184     gyroscope.setLpBandwidth(LPFBW_42HZ);
00185     //Internal sample rate of 200Hz.
00186     gyroscope.setSampleRateDivider(4);
00187 
00188 }
00189 
00190 void IMU::calibrateGyroscope(void) {
00191 
00192     w_xAccumulator = 0;
00193     w_yAccumulator = 0;
00194     w_zAccumulator = 0;
00195 
00196     //Accumulate a certain number of samples.
00197     for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
00198 
00199         w_xAccumulator += gyroscope.getGyroX();
00200         w_yAccumulator += gyroscope.getGyroY();
00201         w_zAccumulator += gyroscope.getGyroZ();
00202         wait(gyroscopeRate_);
00203 
00204     }
00205 
00206     //Average the samples.
00207     w_xAccumulator /= CALIBRATION_SAMPLES;
00208     w_yAccumulator /= CALIBRATION_SAMPLES;
00209     w_zAccumulator /= CALIBRATION_SAMPLES;
00210 
00211     //Set the null bias.
00212     w_xBias = w_xAccumulator;
00213     w_yBias = w_yAccumulator;
00214     w_zBias = w_zAccumulator;
00215 
00216     //Reset the accumulators.
00217     w_xAccumulator = 0;
00218     w_yAccumulator = 0;
00219     w_zAccumulator = 0;
00220 
00221 }
00222 
00223 void IMU::sampleGyroscope(void) {
00224 
00225     //If we've taken the required number of samples then,
00226     //average the samples, removed the null bias and convert the units
00227     //to rad/s.
00228     if (gyroscopeSamples == SAMPLES) {
00229 
00230         w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN);
00231         w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN);
00232         w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN);
00233 
00234         w_xAccumulator = 0;
00235         w_yAccumulator = 0;
00236         w_zAccumulator = 0;
00237         gyroscopeSamples = 0;
00238 
00239     }
00240     //Accumulate another sample. 
00241     else {
00242 
00243         w_xAccumulator += gyroscope.getGyroX();
00244         w_yAccumulator += gyroscope.getGyroY();
00245         w_zAccumulator += gyroscope.getGyroZ();
00246 
00247         gyroscopeSamples++;
00248 
00249     }
00250 
00251 }
00252 
00253 void IMU::sample(void) {
00254 
00255     //If we've taken enough samples then,
00256     //average the samples, remove the offsets and convert to appropriate units.
00257     //Feed this information into the filter to calculate the new Euler angles.
00258     if (accelerometerSamples == SAMPLES) {
00259 
00260         a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN;
00261         a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN;
00262         a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN;
00263 
00264         a_xAccumulator = 0;
00265         a_yAccumulator = 0;
00266         a_zAccumulator = 0;
00267 
00268         accelerometerSamples = 0;
00269 
00270         w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN);
00271         w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN);
00272         w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN);
00273 
00274         w_xAccumulator = 0;
00275         w_yAccumulator = 0;
00276         w_zAccumulator = 0;
00277         gyroscopeSamples = 0;
00278 
00279         //Update the filter variables.
00280         imuFilter.updateFilter(w_y, w_x, w_z, a_y, a_x, a_z);
00281         //Calculate the new Euler angles.
00282         imuFilter.computeEuler();
00283 
00284     }
00285     //Accumulate another sample. 
00286     else {
00287 
00288         accelerometer.getOutput(readings);
00289 
00290         a_xAccumulator += (int16_t) readings[0];
00291         a_yAccumulator += (int16_t) readings[1];
00292         a_zAccumulator += (int16_t) readings[2];
00293 
00294         w_xAccumulator += gyroscope.getGyroX();
00295         w_yAccumulator += gyroscope.getGyroY();
00296         w_zAccumulator += gyroscope.getGyroZ();
00297 
00298         accelerometerSamples++;
00299 
00300     }
00301 
00302 }
00303 
00304 void IMU::filter(void) {
00305 
00306     //Update the filter variables.
00307     imuFilter.updateFilter(w_y, w_x, w_z, a_y, a_x, a_z);
00308     //Calculate the new Euler angles.
00309     imuFilter.computeEuler();
00310 
00311 }
00312 
00313 void IMU::reset(void) {
00314 
00315     //Disable interrupts.
00316     sampleTicker.detach();
00317     
00318     //Recalibrate sensors.
00319     calibrateAccelerometer();
00320     calibrateGyroscope();
00321     
00322     //Reset the IMU filter.
00323     //imuFilter.reset();
00324     
00325     //Reset the working variables.
00326     a_xAccumulator = 0;
00327     a_yAccumulator = 0;
00328     a_zAccumulator = 0;
00329     w_xAccumulator = 0;
00330     w_yAccumulator = 0;
00331     w_zAccumulator = 0;
00332     accelerometerSamples = 0;
00333     gyroscopeSamples = 0;
00334     
00335     //Enable interrupts.
00336     sampleTicker.attach(this, &IMU::sample, accelerometerRate_);
00337 
00338 }