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