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