My modifications/additions to the code
Dependencies: ADXL345 ADXL345_I2C IMUfilter ITG3200 Servo fishgait mbed-rtos mbed pixy_cam
Fork of robotic_fish_ver_4_8 by
IMU.cpp@25:4f2f441eceec, 2014-07-11 (annotated)
- Committer:
- sandwich
- Date:
- Fri Jul 11 14:30:36 2014 +0000
- Revision:
- 25:4f2f441eceec
- Parent:
- 0:ff9bc5f69c57
latest revision
Who changed what in which revision?
User | Revision | Line number | New 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 | } |