Aaron Berk / Mbed 2 deprecated IMU

The IMUfilter Roll/Pitch/Yaw example wrapped up into a class for easier use with other programs: http://mbed.org/users/aberk/programs/IMUfilter_RPYExample/latest

Dependencies:   mbed

IMU.cpp@0:51d01aa47c71, 2010-08-11 (annotated)

Committer:
aberk
Date:
Wed Aug 11 11:09:47 2010 +0000
Revision:
0:51d01aa47c71
Version 1.0

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aberk 0:51d01aa47c71 1 /**
aberk 0:51d01aa47c71 2 * @section LICENSE
aberk 0:51d01aa47c71 3 *
aberk 0:51d01aa47c71 4 * Copyright (c) 2010 ARM Limited
aberk 0:51d01aa47c71 5 *
aberk 0:51d01aa47c71 6 * Permission is hereby granted, free of charge, to any person obtaining a copy
aberk 0:51d01aa47c71 7 * of this software and associated documentation files (the "Software"), to deal
aberk 0:51d01aa47c71 8 * in the Software without restriction, including without limitation the rights
aberk 0:51d01aa47c71 9 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
aberk 0:51d01aa47c71 10 * copies of the Software, and to permit persons to whom the Software is
aberk 0:51d01aa47c71 11 * furnished to do so, subject to the following conditions:
aberk 0:51d01aa47c71 12 *
aberk 0:51d01aa47c71 13 * The above copyright notice and this permission notice shall be included in
aberk 0:51d01aa47c71 14 * all copies or substantial portions of the Software.
aberk 0:51d01aa47c71 15 *
aberk 0:51d01aa47c71 16 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
aberk 0:51d01aa47c71 17 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
aberk 0:51d01aa47c71 18 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
aberk 0:51d01aa47c71 19 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
aberk 0:51d01aa47c71 20 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
aberk 0:51d01aa47c71 21 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
aberk 0:51d01aa47c71 22 * THE SOFTWARE.
aberk 0:51d01aa47c71 23 *
aberk 0:51d01aa47c71 24 * @section DESCRIPTION
aberk 0:51d01aa47c71 25 *
aberk 0:51d01aa47c71 26 * IMU consisting of ADXL345 accelerometer and ITG-3200 gyroscope using
aberk 0:51d01aa47c71 27 * orientation filter developed by Sebastian Madgwick.
aberk 0:51d01aa47c71 28 *
aberk 0:51d01aa47c71 29 * Find more details about his paper here:
aberk 0:51d01aa47c71 30 *
aberk 0:51d01aa47c71 31 * http://code.google.com/p/imumargalgorithm30042010sohm/
aberk 0:51d01aa47c71 32 */
aberk 0:51d01aa47c71 33
aberk 0:51d01aa47c71 34
aberk 0:51d01aa47c71 35 /**
aberk 0:51d01aa47c71 36 * Includes
aberk 0:51d01aa47c71 37 */
aberk 0:51d01aa47c71 38 #include "IMU.h"
aberk 0:51d01aa47c71 39
aberk 0:51d01aa47c71 40 IMU::IMU(float imuRate,
aberk 0:51d01aa47c71 41 double gyroscopeMeasurementError,
aberk 0:51d01aa47c71 42 float accelerometerRate,
aberk 0:51d01aa47c71 43 float gyroscopeRate) : accelerometer(p5, p6, p7, p8),
aberk 0:51d01aa47c71 44 gyroscope(p9, p10), imuFilter(imuRate, gyroscopeMeasurementError) {
aberk 0:51d01aa47c71 45
aberk 0:51d01aa47c71 46 imuRate_ = imuRate;
aberk 0:51d01aa47c71 47 accelerometerRate_ = accelerometerRate;
aberk 0:51d01aa47c71 48 gyroscopeRate_ = gyroscopeRate;
aberk 0:51d01aa47c71 49
aberk 0:51d01aa47c71 50 a_xAccumulator = 0;
aberk 0:51d01aa47c71 51 a_yAccumulator = 0;
aberk 0:51d01aa47c71 52 a_zAccumulator = 0;
aberk 0:51d01aa47c71 53 w_xAccumulator = 0;
aberk 0:51d01aa47c71 54 w_yAccumulator = 0;
aberk 0:51d01aa47c71 55 w_zAccumulator = 0;
aberk 0:51d01aa47c71 56
aberk 0:51d01aa47c71 57 accelerometerSamples = 0;
aberk 0:51d01aa47c71 58 gyroscopeSamples = 0;
aberk 0:51d01aa47c71 59
aberk 0:51d01aa47c71 60 initializeAccelerometer();
aberk 0:51d01aa47c71 61 calibrateAccelerometer();
aberk 0:51d01aa47c71 62
aberk 0:51d01aa47c71 63 initializeGyroscope();
aberk 0:51d01aa47c71 64 calibrateGyroscope();
aberk 0:51d01aa47c71 65
aberk 0:51d01aa47c71 66 accelerometerTicker.attach(this, &IMU::sampleAccelerometer, accelerometerRate_);
aberk 0:51d01aa47c71 67 gyroscopeTicker.attach(this, &IMU::sampleGyroscope, gyroscopeRate_);
aberk 0:51d01aa47c71 68 filterTicker.attach(this, &IMU::filter, imuRate_);
aberk 0:51d01aa47c71 69
aberk 0:51d01aa47c71 70 }
aberk 0:51d01aa47c71 71
aberk 0:51d01aa47c71 72 double IMU::getRoll(void) {
aberk 0:51d01aa47c71 73
aberk 0:51d01aa47c71 74 return toDegrees(imuFilter.getRoll());
aberk 0:51d01aa47c71 75
aberk 0:51d01aa47c71 76 }
aberk 0:51d01aa47c71 77
aberk 0:51d01aa47c71 78 double IMU::getPitch(void) {
aberk 0:51d01aa47c71 79
aberk 0:51d01aa47c71 80 return toDegrees(imuFilter.getPitch());
aberk 0:51d01aa47c71 81
aberk 0:51d01aa47c71 82 }
aberk 0:51d01aa47c71 83
aberk 0:51d01aa47c71 84 double IMU::getYaw(void) {
aberk 0:51d01aa47c71 85
aberk 0:51d01aa47c71 86 return toDegrees(imuFilter.getYaw());
aberk 0:51d01aa47c71 87
aberk 0:51d01aa47c71 88 }
aberk 0:51d01aa47c71 89
aberk 0:51d01aa47c71 90 void IMU::initializeAccelerometer(void) {
aberk 0:51d01aa47c71 91
aberk 0:51d01aa47c71 92 //Go into standby mode to configure the device.
aberk 0:51d01aa47c71 93 accelerometer.setPowerControl(0x00);
aberk 0:51d01aa47c71 94 //Full resolution, +/-16g, 4mg/LSB.
aberk 0:51d01aa47c71 95 accelerometer.setDataFormatControl(0x0B);
aberk 0:51d01aa47c71 96 //200Hz data rate.
aberk 0:51d01aa47c71 97 accelerometer.setDataRate(ADXL345_200HZ);
aberk 0:51d01aa47c71 98 //Measurement mode.
aberk 0:51d01aa47c71 99 accelerometer.setPowerControl(0x08);
aberk 0:51d01aa47c71 100 //See http://www.analog.com/static/imported-files/application_notes/AN-1077.pdf
aberk 0:51d01aa47c71 101 wait_ms(22);
aberk 0:51d01aa47c71 102
aberk 0:51d01aa47c71 103 }
aberk 0:51d01aa47c71 104
aberk 0:51d01aa47c71 105 void IMU::sampleAccelerometer(void) {
aberk 0:51d01aa47c71 106
aberk 0:51d01aa47c71 107 if (accelerometerSamples == SAMPLES) {
aberk 0:51d01aa47c71 108
aberk 0:51d01aa47c71 109 a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN;
aberk 0:51d01aa47c71 110 a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN;
aberk 0:51d01aa47c71 111 a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN;
aberk 0:51d01aa47c71 112
aberk 0:51d01aa47c71 113 a_xAccumulator = 0;
aberk 0:51d01aa47c71 114 a_yAccumulator = 0;
aberk 0:51d01aa47c71 115 a_zAccumulator = 0;
aberk 0:51d01aa47c71 116 accelerometerSamples = 0;
aberk 0:51d01aa47c71 117
aberk 0:51d01aa47c71 118 } else {
aberk 0:51d01aa47c71 119
aberk 0:51d01aa47c71 120 accelerometer.getOutput(readings);
aberk 0:51d01aa47c71 121
aberk 0:51d01aa47c71 122 a_xAccumulator += (int16_t) readings[0];
aberk 0:51d01aa47c71 123 a_yAccumulator += (int16_t) readings[1];
aberk 0:51d01aa47c71 124 a_zAccumulator += (int16_t) readings[2];
aberk 0:51d01aa47c71 125
aberk 0:51d01aa47c71 126 accelerometerSamples++;
aberk 0:51d01aa47c71 127
aberk 0:51d01aa47c71 128 }
aberk 0:51d01aa47c71 129
aberk 0:51d01aa47c71 130 }
aberk 0:51d01aa47c71 131
aberk 0:51d01aa47c71 132 void IMU::calibrateAccelerometer(void) {
aberk 0:51d01aa47c71 133
aberk 0:51d01aa47c71 134 a_xAccumulator = 0;
aberk 0:51d01aa47c71 135 a_yAccumulator = 0;
aberk 0:51d01aa47c71 136 a_zAccumulator = 0;
aberk 0:51d01aa47c71 137
aberk 0:51d01aa47c71 138 for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
aberk 0:51d01aa47c71 139
aberk 0:51d01aa47c71 140 accelerometer.getOutput(readings);
aberk 0:51d01aa47c71 141
aberk 0:51d01aa47c71 142 a_xAccumulator += (int16_t) readings[0];
aberk 0:51d01aa47c71 143 a_yAccumulator += (int16_t) readings[1];
aberk 0:51d01aa47c71 144 a_zAccumulator += (int16_t) readings[2];
aberk 0:51d01aa47c71 145
aberk 0:51d01aa47c71 146 wait(accelerometerRate_);
aberk 0:51d01aa47c71 147
aberk 0:51d01aa47c71 148 }
aberk 0:51d01aa47c71 149
aberk 0:51d01aa47c71 150 a_xAccumulator /= CALIBRATION_SAMPLES;
aberk 0:51d01aa47c71 151 a_yAccumulator /= CALIBRATION_SAMPLES;
aberk 0:51d01aa47c71 152 a_zAccumulator /= CALIBRATION_SAMPLES;
aberk 0:51d01aa47c71 153
aberk 0:51d01aa47c71 154 a_xBias = a_xAccumulator;
aberk 0:51d01aa47c71 155 a_yBias = a_yAccumulator;
aberk 0:51d01aa47c71 156 a_zBias = (a_zAccumulator - 250);
aberk 0:51d01aa47c71 157
aberk 0:51d01aa47c71 158 a_xAccumulator = 0;
aberk 0:51d01aa47c71 159 a_yAccumulator = 0;
aberk 0:51d01aa47c71 160 a_zAccumulator = 0;
aberk 0:51d01aa47c71 161
aberk 0:51d01aa47c71 162 }
aberk 0:51d01aa47c71 163
aberk 0:51d01aa47c71 164 void IMU::initializeGyroscope(void) {
aberk 0:51d01aa47c71 165
aberk 0:51d01aa47c71 166 //Low pass filter bandwidth of 42Hz.
aberk 0:51d01aa47c71 167 gyroscope.setLpBandwidth(LPFBW_42HZ);
aberk 0:51d01aa47c71 168 //Internal sample rate of 200Hz.
aberk 0:51d01aa47c71 169 gyroscope.setSampleRateDivider(4);
aberk 0:51d01aa47c71 170
aberk 0:51d01aa47c71 171 }
aberk 0:51d01aa47c71 172
aberk 0:51d01aa47c71 173 void IMU::calibrateGyroscope(void) {
aberk 0:51d01aa47c71 174
aberk 0:51d01aa47c71 175 w_xAccumulator = 0;
aberk 0:51d01aa47c71 176 w_yAccumulator = 0;
aberk 0:51d01aa47c71 177 w_zAccumulator = 0;
aberk 0:51d01aa47c71 178
aberk 0:51d01aa47c71 179 for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
aberk 0:51d01aa47c71 180
aberk 0:51d01aa47c71 181 w_xAccumulator += gyroscope.getGyroX();
aberk 0:51d01aa47c71 182 w_yAccumulator += gyroscope.getGyroY();
aberk 0:51d01aa47c71 183 w_zAccumulator += gyroscope.getGyroZ();
aberk 0:51d01aa47c71 184 wait(gyroscopeRate_);
aberk 0:51d01aa47c71 185
aberk 0:51d01aa47c71 186 }
aberk 0:51d01aa47c71 187
aberk 0:51d01aa47c71 188 //Average the samples.
aberk 0:51d01aa47c71 189 w_xAccumulator /= CALIBRATION_SAMPLES;
aberk 0:51d01aa47c71 190 w_yAccumulator /= CALIBRATION_SAMPLES;
aberk 0:51d01aa47c71 191 w_zAccumulator /= CALIBRATION_SAMPLES;
aberk 0:51d01aa47c71 192
aberk 0:51d01aa47c71 193 w_xBias = w_xAccumulator;
aberk 0:51d01aa47c71 194 w_yBias = w_yAccumulator;
aberk 0:51d01aa47c71 195 w_zBias = w_zAccumulator;
aberk 0:51d01aa47c71 196
aberk 0:51d01aa47c71 197 w_xAccumulator = 0;
aberk 0:51d01aa47c71 198 w_yAccumulator = 0;
aberk 0:51d01aa47c71 199 w_zAccumulator = 0;
aberk 0:51d01aa47c71 200
aberk 0:51d01aa47c71 201 }
aberk 0:51d01aa47c71 202
aberk 0:51d01aa47c71 203 void IMU::sampleGyroscope(void) {
aberk 0:51d01aa47c71 204
aberk 0:51d01aa47c71 205 if (gyroscopeSamples == SAMPLES) {
aberk 0:51d01aa47c71 206
aberk 0:51d01aa47c71 207 w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN);
aberk 0:51d01aa47c71 208 w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN);
aberk 0:51d01aa47c71 209 w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN);
aberk 0:51d01aa47c71 210
aberk 0:51d01aa47c71 211 w_xAccumulator = 0;
aberk 0:51d01aa47c71 212 w_yAccumulator = 0;
aberk 0:51d01aa47c71 213 w_zAccumulator = 0;
aberk 0:51d01aa47c71 214 gyroscopeSamples = 0;
aberk 0:51d01aa47c71 215
aberk 0:51d01aa47c71 216 } else {
aberk 0:51d01aa47c71 217
aberk 0:51d01aa47c71 218 w_xAccumulator += gyroscope.getGyroX();
aberk 0:51d01aa47c71 219 w_yAccumulator += gyroscope.getGyroY();
aberk 0:51d01aa47c71 220 w_zAccumulator += gyroscope.getGyroZ();
aberk 0:51d01aa47c71 221
aberk 0:51d01aa47c71 222 gyroscopeSamples++;
aberk 0:51d01aa47c71 223
aberk 0:51d01aa47c71 224 }
aberk 0:51d01aa47c71 225
aberk 0:51d01aa47c71 226 }
aberk 0:51d01aa47c71 227
aberk 0:51d01aa47c71 228 void IMU::filter(void) {
aberk 0:51d01aa47c71 229
aberk 0:51d01aa47c71 230 //Update the filter variables.
aberk 0:51d01aa47c71 231 imuFilter.updateFilter(w_y, w_x, w_z, a_y, a_x, a_z);
aberk 0:51d01aa47c71 232 //Calculate the new Euler angles.
aberk 0:51d01aa47c71 233 imuFilter.computeEuler();
aberk 0:51d01aa47c71 234
aberk 0:51d01aa47c71 235 }