Aaron Berk / Mbed 2 deprecated RS-EDP-RDS-Rover

Roving robot using the RS-EDP.

Dependencies:   mbed RSEDP_AM_MC1_lib SDFileSystem

IMU.cpp@1:ffef6386027b, 2010-08-26 (annotated)

Committer:
aberk
Date:
Thu Aug 26 14:41:08 2010 +0000
Revision:
1:ffef6386027b
Added additional comments and documentation.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aberk 1:ffef6386027b 1 /**
aberk 1:ffef6386027b 2 * @section LICENSE
aberk 1:ffef6386027b 3 *
aberk 1:ffef6386027b 4 * Copyright (c) 2010 ARM Limited
aberk 1:ffef6386027b 5 *
aberk 1:ffef6386027b 6 * Permission is hereby granted, free of charge, to any person obtaining a copy
aberk 1:ffef6386027b 7 * of this software and associated documentation files (the "Software"), to deal
aberk 1:ffef6386027b 8 * in the Software without restriction, including without limitation the rights
aberk 1:ffef6386027b 9 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
aberk 1:ffef6386027b 10 * copies of the Software, and to permit persons to whom the Software is
aberk 1:ffef6386027b 11 * furnished to do so, subject to the following conditions:
aberk 1:ffef6386027b 12 *
aberk 1:ffef6386027b 13 * The above copyright notice and this permission notice shall be included in
aberk 1:ffef6386027b 14 * all copies or substantial portions of the Software.
aberk 1:ffef6386027b 15 *
aberk 1:ffef6386027b 16 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
aberk 1:ffef6386027b 17 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
aberk 1:ffef6386027b 18 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
aberk 1:ffef6386027b 19 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
aberk 1:ffef6386027b 20 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
aberk 1:ffef6386027b 21 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
aberk 1:ffef6386027b 22 * THE SOFTWARE.
aberk 1:ffef6386027b 23 *
aberk 1:ffef6386027b 24 * @section DESCRIPTION
aberk 1:ffef6386027b 25 *
aberk 1:ffef6386027b 26 * IMU consisting of ADXL345 accelerometer and ITG-3200 gyroscope using
aberk 1:ffef6386027b 27 * orientation filter developed by Sebastian Madgwick.
aberk 1:ffef6386027b 28 *
aberk 1:ffef6386027b 29 * Find more details about his paper here:
aberk 1:ffef6386027b 30 *
aberk 1:ffef6386027b 31 * http://code.google.com/p/imumargalgorithm30042010sohm/
aberk 1:ffef6386027b 32 */
aberk 1:ffef6386027b 33
aberk 1:ffef6386027b 34
aberk 1:ffef6386027b 35 /**
aberk 1:ffef6386027b 36 * Includes
aberk 1:ffef6386027b 37 */
aberk 1:ffef6386027b 38 #include "IMU.h"
aberk 1:ffef6386027b 39
aberk 1:ffef6386027b 40 IMU::IMU(float imuRate,
aberk 1:ffef6386027b 41 double gyroscopeMeasurementError,
aberk 1:ffef6386027b 42 float accelerometerRate,
aberk 1:ffef6386027b 43 float gyroscopeRate) : accelerometer(p5, p6, p7, p8),
aberk 1:ffef6386027b 44 gyroscope(p9, p10), imuFilter(imuRate, gyroscopeMeasurementError) {
aberk 1:ffef6386027b 45
aberk 1:ffef6386027b 46 imuRate_ = imuRate;
aberk 1:ffef6386027b 47 accelerometerRate_ = accelerometerRate;
aberk 1:ffef6386027b 48 gyroscopeRate_ = gyroscopeRate;
aberk 1:ffef6386027b 49
aberk 1:ffef6386027b 50 //Initialize sampling variables.
aberk 1:ffef6386027b 51 a_xAccumulator = 0;
aberk 1:ffef6386027b 52 a_yAccumulator = 0;
aberk 1:ffef6386027b 53 a_zAccumulator = 0;
aberk 1:ffef6386027b 54 w_xAccumulator = 0;
aberk 1:ffef6386027b 55 w_yAccumulator = 0;
aberk 1:ffef6386027b 56 w_zAccumulator = 0;
aberk 1:ffef6386027b 57
aberk 1:ffef6386027b 58 accelerometerSamples = 0;
aberk 1:ffef6386027b 59 gyroscopeSamples = 0;
aberk 1:ffef6386027b 60
aberk 1:ffef6386027b 61 //Initialize and calibrate sensors.
aberk 1:ffef6386027b 62 initializeAccelerometer();
aberk 1:ffef6386027b 63 calibrateAccelerometer();
aberk 1:ffef6386027b 64
aberk 1:ffef6386027b 65 initializeGyroscope();
aberk 1:ffef6386027b 66 calibrateGyroscope();
aberk 1:ffef6386027b 67
aberk 1:ffef6386027b 68 //To reduce the number of interrupts we'll remove the separate tickers for
aberk 1:ffef6386027b 69 //the accelerometer, gyro and filter update and combine them all into one.
aberk 1:ffef6386027b 70
aberk 1:ffef6386027b 71 //accelerometerTicker.attach(this, &IMU::sampleAccelerometer, accelerometerRate_);
aberk 1:ffef6386027b 72 //gyroscopeTicker.attach(this, &IMU::sampleGyroscope, gyroscopeRate_);
aberk 1:ffef6386027b 73 sampleTicker.attach(this, &IMU::sample, accelerometerRate_);
aberk 1:ffef6386027b 74 //filterTicker.attach(this, &IMU::filter, imuRate_);
aberk 1:ffef6386027b 75
aberk 1:ffef6386027b 76 }
aberk 1:ffef6386027b 77
aberk 1:ffef6386027b 78 double IMU::getRoll(void) {
aberk 1:ffef6386027b 79
aberk 1:ffef6386027b 80 return toDegrees(imuFilter.getRoll());
aberk 1:ffef6386027b 81
aberk 1:ffef6386027b 82 }
aberk 1:ffef6386027b 83
aberk 1:ffef6386027b 84 double IMU::getPitch(void) {
aberk 1:ffef6386027b 85
aberk 1:ffef6386027b 86 return toDegrees(imuFilter.getPitch());
aberk 1:ffef6386027b 87
aberk 1:ffef6386027b 88 }
aberk 1:ffef6386027b 89
aberk 1:ffef6386027b 90 double IMU::getYaw(void) {
aberk 1:ffef6386027b 91
aberk 1:ffef6386027b 92 return toDegrees(imuFilter.getYaw());
aberk 1:ffef6386027b 93
aberk 1:ffef6386027b 94 }
aberk 1:ffef6386027b 95
aberk 1:ffef6386027b 96 void IMU::initializeAccelerometer(void) {
aberk 1:ffef6386027b 97
aberk 1:ffef6386027b 98 //Go into standby mode to configure the device.
aberk 1:ffef6386027b 99 accelerometer.setPowerControl(0x00);
aberk 1:ffef6386027b 100 //Full resolution, +/-16g, 4mg/LSB.
aberk 1:ffef6386027b 101 accelerometer.setDataFormatControl(0x0B);
aberk 1:ffef6386027b 102 //200Hz data rate.
aberk 1:ffef6386027b 103 accelerometer.setDataRate(ADXL345_200HZ);
aberk 1:ffef6386027b 104 //Measurement mode.
aberk 1:ffef6386027b 105 accelerometer.setPowerControl(0x08);
aberk 1:ffef6386027b 106 //See http://www.analog.com/static/imported-files/application_notes/AN-1077.pdf
aberk 1:ffef6386027b 107 wait_ms(22);
aberk 1:ffef6386027b 108
aberk 1:ffef6386027b 109 }
aberk 1:ffef6386027b 110
aberk 1:ffef6386027b 111 void IMU::sampleAccelerometer(void) {
aberk 1:ffef6386027b 112
aberk 1:ffef6386027b 113 //If we've taken a certain number of samples,
aberk 1:ffef6386027b 114 //average them, remove the bias and convert the units.
aberk 1:ffef6386027b 115 if (accelerometerSamples == SAMPLES) {
aberk 1:ffef6386027b 116
aberk 1:ffef6386027b 117 a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN;
aberk 1:ffef6386027b 118 a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN;
aberk 1:ffef6386027b 119 a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN;
aberk 1:ffef6386027b 120
aberk 1:ffef6386027b 121 a_xAccumulator = 0;
aberk 1:ffef6386027b 122 a_yAccumulator = 0;
aberk 1:ffef6386027b 123 a_zAccumulator = 0;
aberk 1:ffef6386027b 124 accelerometerSamples = 0;
aberk 1:ffef6386027b 125
aberk 1:ffef6386027b 126 }
aberk 1:ffef6386027b 127 //Otherwise, accumulate another reading.
aberk 1:ffef6386027b 128 else {
aberk 1:ffef6386027b 129
aberk 1:ffef6386027b 130 accelerometer.getOutput(readings);
aberk 1:ffef6386027b 131
aberk 1:ffef6386027b 132 a_xAccumulator += (int16_t) readings[0];
aberk 1:ffef6386027b 133 a_yAccumulator += (int16_t) readings[1];
aberk 1:ffef6386027b 134 a_zAccumulator += (int16_t) readings[2];
aberk 1:ffef6386027b 135
aberk 1:ffef6386027b 136 accelerometerSamples++;
aberk 1:ffef6386027b 137
aberk 1:ffef6386027b 138 }
aberk 1:ffef6386027b 139
aberk 1:ffef6386027b 140 }
aberk 1:ffef6386027b 141
aberk 1:ffef6386027b 142 void IMU::calibrateAccelerometer(void) {
aberk 1:ffef6386027b 143
aberk 1:ffef6386027b 144 a_xAccumulator = 0;
aberk 1:ffef6386027b 145 a_yAccumulator = 0;
aberk 1:ffef6386027b 146 a_zAccumulator = 0;
aberk 1:ffef6386027b 147
aberk 1:ffef6386027b 148 //Accumulate a certain number of samples.
aberk 1:ffef6386027b 149 for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
aberk 1:ffef6386027b 150
aberk 1:ffef6386027b 151 accelerometer.getOutput(readings);
aberk 1:ffef6386027b 152
aberk 1:ffef6386027b 153 a_xAccumulator += (int16_t) readings[0];
aberk 1:ffef6386027b 154 a_yAccumulator += (int16_t) readings[1];
aberk 1:ffef6386027b 155 a_zAccumulator += (int16_t) readings[2];
aberk 1:ffef6386027b 156
aberk 1:ffef6386027b 157 wait(accelerometerRate_);
aberk 1:ffef6386027b 158
aberk 1:ffef6386027b 159 }
aberk 1:ffef6386027b 160
aberk 1:ffef6386027b 161 //Average the samples.
aberk 1:ffef6386027b 162 a_xAccumulator /= CALIBRATION_SAMPLES;
aberk 1:ffef6386027b 163 a_yAccumulator /= CALIBRATION_SAMPLES;
aberk 1:ffef6386027b 164 a_zAccumulator /= CALIBRATION_SAMPLES;
aberk 1:ffef6386027b 165
aberk 1:ffef6386027b 166 //These are our zero g offsets.
aberk 1:ffef6386027b 167 //250 = 9.81m/s/s @ 4mg/LSB.
aberk 1:ffef6386027b 168 a_xBias = a_xAccumulator;
aberk 1:ffef6386027b 169 a_yBias = a_yAccumulator;
aberk 1:ffef6386027b 170 a_zBias = (a_zAccumulator - 250);
aberk 1:ffef6386027b 171
aberk 1:ffef6386027b 172 //Reset accumulators.
aberk 1:ffef6386027b 173 a_xAccumulator = 0;
aberk 1:ffef6386027b 174 a_yAccumulator = 0;
aberk 1:ffef6386027b 175 a_zAccumulator = 0;
aberk 1:ffef6386027b 176
aberk 1:ffef6386027b 177 }
aberk 1:ffef6386027b 178
aberk 1:ffef6386027b 179 void IMU::initializeGyroscope(void) {
aberk 1:ffef6386027b 180
aberk 1:ffef6386027b 181 //Low pass filter bandwidth of 42Hz.
aberk 1:ffef6386027b 182 gyroscope.setLpBandwidth(LPFBW_42HZ);
aberk 1:ffef6386027b 183 //Internal sample rate of 200Hz.
aberk 1:ffef6386027b 184 gyroscope.setSampleRateDivider(4);
aberk 1:ffef6386027b 185
aberk 1:ffef6386027b 186 }
aberk 1:ffef6386027b 187
aberk 1:ffef6386027b 188 void IMU::calibrateGyroscope(void) {
aberk 1:ffef6386027b 189
aberk 1:ffef6386027b 190 w_xAccumulator = 0;
aberk 1:ffef6386027b 191 w_yAccumulator = 0;
aberk 1:ffef6386027b 192 w_zAccumulator = 0;
aberk 1:ffef6386027b 193
aberk 1:ffef6386027b 194 //Accumulate a certain number of samples.
aberk 1:ffef6386027b 195 for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
aberk 1:ffef6386027b 196
aberk 1:ffef6386027b 197 w_xAccumulator += gyroscope.getGyroX();
aberk 1:ffef6386027b 198 w_yAccumulator += gyroscope.getGyroY();
aberk 1:ffef6386027b 199 w_zAccumulator += gyroscope.getGyroZ();
aberk 1:ffef6386027b 200 wait(gyroscopeRate_);
aberk 1:ffef6386027b 201
aberk 1:ffef6386027b 202 }
aberk 1:ffef6386027b 203
aberk 1:ffef6386027b 204 //Average the samples.
aberk 1:ffef6386027b 205 w_xAccumulator /= CALIBRATION_SAMPLES;
aberk 1:ffef6386027b 206 w_yAccumulator /= CALIBRATION_SAMPLES;
aberk 1:ffef6386027b 207 w_zAccumulator /= CALIBRATION_SAMPLES;
aberk 1:ffef6386027b 208
aberk 1:ffef6386027b 209 //Set the null bias.
aberk 1:ffef6386027b 210 w_xBias = w_xAccumulator;
aberk 1:ffef6386027b 211 w_yBias = w_yAccumulator;
aberk 1:ffef6386027b 212 w_zBias = w_zAccumulator;
aberk 1:ffef6386027b 213
aberk 1:ffef6386027b 214 //Reset the accumulators.
aberk 1:ffef6386027b 215 w_xAccumulator = 0;
aberk 1:ffef6386027b 216 w_yAccumulator = 0;
aberk 1:ffef6386027b 217 w_zAccumulator = 0;
aberk 1:ffef6386027b 218
aberk 1:ffef6386027b 219 }
aberk 1:ffef6386027b 220
aberk 1:ffef6386027b 221 void IMU::sampleGyroscope(void) {
aberk 1:ffef6386027b 222
aberk 1:ffef6386027b 223 //If we've taken the required number of samples then,
aberk 1:ffef6386027b 224 //average the samples, removed the null bias and convert the units
aberk 1:ffef6386027b 225 //to rad/s.
aberk 1:ffef6386027b 226 if (gyroscopeSamples == SAMPLES) {
aberk 1:ffef6386027b 227
aberk 1:ffef6386027b 228 w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN);
aberk 1:ffef6386027b 229 w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN);
aberk 1:ffef6386027b 230 w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN);
aberk 1:ffef6386027b 231
aberk 1:ffef6386027b 232 w_xAccumulator = 0;
aberk 1:ffef6386027b 233 w_yAccumulator = 0;
aberk 1:ffef6386027b 234 w_zAccumulator = 0;
aberk 1:ffef6386027b 235 gyroscopeSamples = 0;
aberk 1:ffef6386027b 236
aberk 1:ffef6386027b 237 }
aberk 1:ffef6386027b 238 //Accumulate another sample.
aberk 1:ffef6386027b 239 else {
aberk 1:ffef6386027b 240
aberk 1:ffef6386027b 241 w_xAccumulator += gyroscope.getGyroX();
aberk 1:ffef6386027b 242 w_yAccumulator += gyroscope.getGyroY();
aberk 1:ffef6386027b 243 w_zAccumulator += gyroscope.getGyroZ();
aberk 1:ffef6386027b 244
aberk 1:ffef6386027b 245 gyroscopeSamples++;
aberk 1:ffef6386027b 246
aberk 1:ffef6386027b 247 }
aberk 1:ffef6386027b 248
aberk 1:ffef6386027b 249 }
aberk 1:ffef6386027b 250
aberk 1:ffef6386027b 251 void IMU::sample(void) {
aberk 1:ffef6386027b 252
aberk 1:ffef6386027b 253 //If we've taken enough samples then,
aberk 1:ffef6386027b 254 //average the samples, remove the offsets and convert to appropriate units.
aberk 1:ffef6386027b 255 //Feed this information into the filter to calculate the new Euler angles.
aberk 1:ffef6386027b 256 if (accelerometerSamples == SAMPLES) {
aberk 1:ffef6386027b 257
aberk 1:ffef6386027b 258 a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN;
aberk 1:ffef6386027b 259 a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN;
aberk 1:ffef6386027b 260 a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN;
aberk 1:ffef6386027b 261
aberk 1:ffef6386027b 262 a_xAccumulator = 0;
aberk 1:ffef6386027b 263 a_yAccumulator = 0;
aberk 1:ffef6386027b 264 a_zAccumulator = 0;
aberk 1:ffef6386027b 265
aberk 1:ffef6386027b 266 accelerometerSamples = 0;
aberk 1:ffef6386027b 267
aberk 1:ffef6386027b 268 w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN);
aberk 1:ffef6386027b 269 w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN);
aberk 1:ffef6386027b 270 w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN);
aberk 1:ffef6386027b 271
aberk 1:ffef6386027b 272 w_xAccumulator = 0;
aberk 1:ffef6386027b 273 w_yAccumulator = 0;
aberk 1:ffef6386027b 274 w_zAccumulator = 0;
aberk 1:ffef6386027b 275 gyroscopeSamples = 0;
aberk 1:ffef6386027b 276
aberk 1:ffef6386027b 277 //Update the filter variables.
aberk 1:ffef6386027b 278 imuFilter.updateFilter(w_y, w_x, w_z, a_y, a_x, a_z);
aberk 1:ffef6386027b 279 //Calculate the new Euler angles.
aberk 1:ffef6386027b 280 imuFilter.computeEuler();
aberk 1:ffef6386027b 281
aberk 1:ffef6386027b 282 }
aberk 1:ffef6386027b 283 //Accumulate another sample.
aberk 1:ffef6386027b 284 else {
aberk 1:ffef6386027b 285
aberk 1:ffef6386027b 286 accelerometer.getOutput(readings);
aberk 1:ffef6386027b 287
aberk 1:ffef6386027b 288 a_xAccumulator += (int16_t) readings[0];
aberk 1:ffef6386027b 289 a_yAccumulator += (int16_t) readings[1];
aberk 1:ffef6386027b 290 a_zAccumulator += (int16_t) readings[2];
aberk 1:ffef6386027b 291
aberk 1:ffef6386027b 292 w_xAccumulator += gyroscope.getGyroX();
aberk 1:ffef6386027b 293 w_yAccumulator += gyroscope.getGyroY();
aberk 1:ffef6386027b 294 w_zAccumulator += gyroscope.getGyroZ();
aberk 1:ffef6386027b 295
aberk 1:ffef6386027b 296 accelerometerSamples++;
aberk 1:ffef6386027b 297
aberk 1:ffef6386027b 298 }
aberk 1:ffef6386027b 299
aberk 1:ffef6386027b 300 }
aberk 1:ffef6386027b 301
aberk 1:ffef6386027b 302 void IMU::filter(void) {
aberk 1:ffef6386027b 303
aberk 1:ffef6386027b 304 //Update the filter variables.
aberk 1:ffef6386027b 305 imuFilter.updateFilter(w_y, w_x, w_z, a_y, a_x, a_z);
aberk 1:ffef6386027b 306 //Calculate the new Euler angles.
aberk 1:ffef6386027b 307 imuFilter.computeEuler();
aberk 1:ffef6386027b 308
aberk 1:ffef6386027b 309 }
aberk 1:ffef6386027b 310
aberk 1:ffef6386027b 311 void IMU::reset(void) {
aberk 1:ffef6386027b 312
aberk 1:ffef6386027b 313 //Disable interrupts.
aberk 1:ffef6386027b 314 sampleTicker.detach();
aberk 1:ffef6386027b 315
aberk 1:ffef6386027b 316 //Recalibrate sensors.
aberk 1:ffef6386027b 317 calibrateAccelerometer();
aberk 1:ffef6386027b 318 calibrateGyroscope();
aberk 1:ffef6386027b 319
aberk 1:ffef6386027b 320 //Reset the IMU filter.
aberk 1:ffef6386027b 321 imuFilter.reset();
aberk 1:ffef6386027b 322
aberk 1:ffef6386027b 323 //Reset the working variables.
aberk 1:ffef6386027b 324 a_xAccumulator = 0;
aberk 1:ffef6386027b 325 a_yAccumulator = 0;
aberk 1:ffef6386027b 326 a_zAccumulator = 0;
aberk 1:ffef6386027b 327 w_xAccumulator = 0;
aberk 1:ffef6386027b 328 w_yAccumulator = 0;
aberk 1:ffef6386027b 329 w_zAccumulator = 0;
aberk 1:ffef6386027b 330 accelerometerSamples = 0;
aberk 1:ffef6386027b 331 gyroscopeSamples = 0;
aberk 1:ffef6386027b 332
aberk 1:ffef6386027b 333 //Enable interrupts.
aberk 1:ffef6386027b 334 sampleTicker.attach(this, &IMU::sample, accelerometerRate_);
aberk 1:ffef6386027b 335
aberk 1:ffef6386027b 336 }