jetfishteam / Mbed 2 deprecated jetfish

the fish that looks like a jet

Dependencies:   ADXL345 ADXL345_I2C IMUfilter ITG3200 mbed Servo

IMU.cpp@16:79cfe6201318, 2014-02-17 (annotated)

Committer:
rkk
Date:
Mon Feb 17 14:04:09 2014 +0000
Revision:
16:79cfe6201318
Parent:
0:ff9bc5f69c57 
changed p5 to p7

Who changed what in which revision?

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