File content as of revision 25:4f2f441eceec:

/**
 * @section LICENSE
 *
 * Copyright (c) 2010 ARM Limited
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 *
 * @section DESCRIPTION
 *
 * IMU consisting of ADXL345 accelerometer and ITG-3200 gyroscope using
 * orientation filter developed by Sebastian Madgwick.
 *
 * Find more details about his paper here:
 *
 * http://code.google.com/p/imumargalgorithm30042010sohm/ 
 */
 
 
/**
 * Includes
 */
#include "IMU.h"
 
IMU::IMU(float imuRate,
         double gyroscopeMeasurementError,
         float accelerometerRate,
         float gyroscopeRate) : accelerometer(p9, p10), 
         gyroscope(p9, p10), imuFilter(imuRate, gyroscopeMeasurementError) {
 
    imuRate_ = imuRate;
    accelerometerRate_ = accelerometerRate;
    gyroscopeRate_ = gyroscopeRate;
 
    a_xAccumulator = 0;
    a_yAccumulator = 0;
    a_zAccumulator = 0;
    w_xAccumulator = 0;
    w_yAccumulator = 0;
    w_zAccumulator = 0;
 
    accelerometerSamples = 0;
    gyroscopeSamples = 0;
 
    initializeAccelerometer();
    calibrateAccelerometer();
 
    initializeGyroscope();
    calibrateGyroscope();
 
    accelerometerTicker.attach(this, &IMU::sampleAccelerometer, accelerometerRate_);
    gyroscopeTicker.attach(this, &IMU::sampleGyroscope, gyroscopeRate_);
    filterTicker.attach(this, &IMU::filter, imuRate_);
 
}
 
double IMU::getRoll(void) {
 
    return toDegrees(imuFilter.getRoll());
 
}
 
double IMU::getPitch(void) {
 
    return toDegrees(imuFilter.getPitch());
 
}
 
double IMU::getYaw(void) {
 
    return toDegrees(imuFilter.getYaw());
 
}
 
void IMU::initializeAccelerometer(void) {
 
    //Go into standby mode to configure the device.
    accelerometer.setPowerControl(0x00);
    //Full resolution, +/-16g, 4mg/LSB.
    accelerometer.setDataFormatControl(0x0B);
    //200Hz data rate.
    accelerometer.setDataRate(ADXL345_200HZ);
    //Measurement mode.
    accelerometer.setPowerControl(0x08);
    //See http://www.analog.com/static/imported-files/application_notes/AN-1077.pdf
    wait_ms(22);
 
}
 
void IMU::sampleAccelerometer(void) {
 
    if (accelerometerSamples == SAMPLES) {
 
        a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN;
        a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN;
        a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN;
 
        a_xAccumulator = 0;
        a_yAccumulator = 0;
        a_zAccumulator = 0;
        accelerometerSamples = 0;
 
    } else {
 
        accelerometer.getOutput(readings);
 
        a_xAccumulator += (int16_t) readings[0];
        a_yAccumulator += (int16_t) readings[1];
        a_zAccumulator += (int16_t) readings[2];
 
        accelerometerSamples++;
 
    }
 
}
 
void IMU::calibrateAccelerometer(void) {
 
    a_xAccumulator = 0;
    a_yAccumulator = 0;
    a_zAccumulator = 0;
 
    for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
 
        accelerometer.getOutput(readings);
 
        a_xAccumulator += (int16_t) readings[0];
        a_yAccumulator += (int16_t) readings[1];
        a_zAccumulator += (int16_t) readings[2];
 
        wait(accelerometerRate_);
 
    }
 
    a_xAccumulator /= CALIBRATION_SAMPLES;
    a_yAccumulator /= CALIBRATION_SAMPLES;
    a_zAccumulator /= CALIBRATION_SAMPLES;
 
    a_xBias = a_xAccumulator;
    a_yBias = a_yAccumulator;
    a_zBias = (a_zAccumulator - 250);
 
    a_xAccumulator = 0;
    a_yAccumulator = 0;
    a_zAccumulator = 0;
 
}
 
void IMU::initializeGyroscope(void) {
 
    //Low pass filter bandwidth of 42Hz.
    gyroscope.setLpBandwidth(LPFBW_42HZ);
    //Internal sample rate of 200Hz.
    gyroscope.setSampleRateDivider(4);
 
}
 
void IMU::calibrateGyroscope(void) {
 
    w_xAccumulator = 0;
    w_yAccumulator = 0;
    w_zAccumulator = 0;
 
    for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
 
        w_xAccumulator += gyroscope.getGyroX();
        w_yAccumulator += gyroscope.getGyroY();
        w_zAccumulator += gyroscope.getGyroZ();
        wait(gyroscopeRate_);
 
    }
 
    //Average the samples.
    w_xAccumulator /= CALIBRATION_SAMPLES;
    w_yAccumulator /= CALIBRATION_SAMPLES;
    w_zAccumulator /= CALIBRATION_SAMPLES;
 
    w_xBias = w_xAccumulator;
    w_yBias = w_yAccumulator;
    w_zBias = w_zAccumulator;
 
    w_xAccumulator = 0;
    w_yAccumulator = 0;
    w_zAccumulator = 0;
 
}
 
void IMU::sampleGyroscope(void) {
 
    if (gyroscopeSamples == SAMPLES) {
 
        w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN);
        w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN);
        w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN);
 
        w_xAccumulator = 0;
        w_yAccumulator = 0;
        w_zAccumulator = 0;
        gyroscopeSamples = 0;
 
    } else {
 
        w_xAccumulator += gyroscope.getGyroX();
        w_yAccumulator += gyroscope.getGyroY();
        w_zAccumulator += gyroscope.getGyroZ();
 
        gyroscopeSamples++;
 
    }
 
}
 
void IMU::filter(void) {
 
    //Update the filter variables.
    imuFilter.updateFilter(w_y, w_x, w_z, a_y, a_x, a_z);
    //Calculate the new Euler angles.
    imuFilter.computeEuler();
 
}