![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
df
Dependencies: mbed ADXL345 IMUfilter ITG3200
main.cpp@1:dda873ab579f, 2010-11-27 (annotated)
- Committer:
- aberk
- Date:
- Sat Nov 27 12:07:45 2010 +0000
- Revision:
- 1:dda873ab579f
- Parent:
- 0:056706c688ae
Changed imports to libraries (from programs).
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
aberk | 0:056706c688ae | 1 | /** |
aberk | 0:056706c688ae | 2 | * IMU filter example. |
aberk | 0:056706c688ae | 3 | * |
aberk | 0:056706c688ae | 4 | * Calculate the roll, pitch and yaw angles. |
aberk | 0:056706c688ae | 5 | */ |
aberk | 0:056706c688ae | 6 | #include "IMUfilter.h" |
aberk | 0:056706c688ae | 7 | #include "ADXL345.h" |
aberk | 0:056706c688ae | 8 | #include "ITG3200.h" |
aberk | 0:056706c688ae | 9 | |
aberk | 0:056706c688ae | 10 | //Gravity at Earth's surface in m/s/s |
aberk | 0:056706c688ae | 11 | #define g0 9.812865328 |
aberk | 0:056706c688ae | 12 | //Number of samples to average. |
aberk | 0:056706c688ae | 13 | #define SAMPLES 4 |
aberk | 0:056706c688ae | 14 | //Number of samples to be averaged for a null bias calculation |
aberk | 0:056706c688ae | 15 | //during calibration. |
aberk | 0:056706c688ae | 16 | #define CALIBRATION_SAMPLES 128 |
aberk | 0:056706c688ae | 17 | //Convert from radians to degrees. |
aberk | 0:056706c688ae | 18 | #define toDegrees(x) (x * 57.2957795) |
aberk | 0:056706c688ae | 19 | //Convert from degrees to radians. |
aberk | 0:056706c688ae | 20 | #define toRadians(x) (x * 0.01745329252) |
aberk | 0:056706c688ae | 21 | //ITG-3200 sensitivity is 14.375 LSB/(degrees/sec). |
aberk | 0:056706c688ae | 22 | #define GYROSCOPE_GAIN (1 / 14.375) |
aberk | 0:056706c688ae | 23 | //Full scale resolution on the ADXL345 is 4mg/LSB. |
aberk | 0:056706c688ae | 24 | #define ACCELEROMETER_GAIN (0.004 * g0) |
aberk | 0:056706c688ae | 25 | //Sampling gyroscope at 200Hz. |
aberk | 0:056706c688ae | 26 | #define GYRO_RATE 0.005 |
aberk | 0:056706c688ae | 27 | //Sampling accelerometer at 200Hz. |
aberk | 0:056706c688ae | 28 | #define ACC_RATE 0.005 |
aberk | 0:056706c688ae | 29 | //Updating filter at 40Hz. |
aberk | 1:dda873ab579f | 30 | #define FILTER_RATE 0.1 |
aberk | 0:056706c688ae | 31 | |
aberk | 0:056706c688ae | 32 | Serial pc(USBTX, USBRX); |
aberk | 0:056706c688ae | 33 | //At rest the gyroscope is centred around 0 and goes between about |
aberk | 0:056706c688ae | 34 | //-5 and 5 counts. As 1 degrees/sec is ~15 LSB, error is roughly |
aberk | 0:056706c688ae | 35 | //5/15 = 0.3 degrees/sec. |
aberk | 0:056706c688ae | 36 | IMUfilter imuFilter(FILTER_RATE, 0.3); |
aberk | 0:056706c688ae | 37 | ADXL345 accelerometer(p5, p6, p7, p8); |
aberk | 0:056706c688ae | 38 | ITG3200 gyroscope(p9, p10); |
aberk | 0:056706c688ae | 39 | Ticker accelerometerTicker; |
aberk | 0:056706c688ae | 40 | Ticker gyroscopeTicker; |
aberk | 0:056706c688ae | 41 | Ticker filterTicker; |
aberk | 0:056706c688ae | 42 | |
aberk | 0:056706c688ae | 43 | //Offsets for the gyroscope. |
aberk | 0:056706c688ae | 44 | //The readings we take when the gyroscope is stationary won't be 0, so we'll |
aberk | 0:056706c688ae | 45 | //average a set of readings we do get when the gyroscope is stationary and |
aberk | 0:056706c688ae | 46 | //take those away from subsequent readings to ensure the gyroscope is offset |
aberk | 0:056706c688ae | 47 | //or "biased" to 0. |
aberk | 0:056706c688ae | 48 | double w_xBias; |
aberk | 0:056706c688ae | 49 | double w_yBias; |
aberk | 0:056706c688ae | 50 | double w_zBias; |
aberk | 0:056706c688ae | 51 | |
aberk | 0:056706c688ae | 52 | //Offsets for the accelerometer. |
aberk | 0:056706c688ae | 53 | //Same as with the gyroscope. |
aberk | 0:056706c688ae | 54 | double a_xBias; |
aberk | 0:056706c688ae | 55 | double a_yBias; |
aberk | 0:056706c688ae | 56 | double a_zBias; |
aberk | 0:056706c688ae | 57 | |
aberk | 0:056706c688ae | 58 | //Accumulators used for oversampling and then averaging. |
aberk | 0:056706c688ae | 59 | volatile double a_xAccumulator = 0; |
aberk | 0:056706c688ae | 60 | volatile double a_yAccumulator = 0; |
aberk | 0:056706c688ae | 61 | volatile double a_zAccumulator = 0; |
aberk | 0:056706c688ae | 62 | volatile double w_xAccumulator = 0; |
aberk | 0:056706c688ae | 63 | volatile double w_yAccumulator = 0; |
aberk | 0:056706c688ae | 64 | volatile double w_zAccumulator = 0; |
aberk | 0:056706c688ae | 65 | |
aberk | 0:056706c688ae | 66 | //Accelerometer and gyroscope readings for x, y, z axes. |
aberk | 0:056706c688ae | 67 | volatile double a_x; |
aberk | 0:056706c688ae | 68 | volatile double a_y; |
aberk | 0:056706c688ae | 69 | volatile double a_z; |
aberk | 0:056706c688ae | 70 | volatile double w_x; |
aberk | 0:056706c688ae | 71 | volatile double w_y; |
aberk | 0:056706c688ae | 72 | volatile double w_z; |
aberk | 0:056706c688ae | 73 | |
aberk | 0:056706c688ae | 74 | //Buffer for accelerometer readings. |
aberk | 0:056706c688ae | 75 | int readings[3]; |
aberk | 0:056706c688ae | 76 | //Number of accelerometer samples we're on. |
aberk | 0:056706c688ae | 77 | int accelerometerSamples = 0; |
aberk | 0:056706c688ae | 78 | //Number of gyroscope samples we're on. |
aberk | 0:056706c688ae | 79 | int gyroscopeSamples = 0; |
aberk | 0:056706c688ae | 80 | |
aberk | 0:056706c688ae | 81 | /** |
aberk | 0:056706c688ae | 82 | * Prototypes |
aberk | 0:056706c688ae | 83 | */ |
aberk | 0:056706c688ae | 84 | //Set up the ADXL345 appropriately. |
aberk | 0:056706c688ae | 85 | void initializeAcceleromter(void); |
aberk | 0:056706c688ae | 86 | //Calculate the null bias. |
aberk | 0:056706c688ae | 87 | void calibrateAccelerometer(void); |
aberk | 0:056706c688ae | 88 | //Take a set of samples and average them. |
aberk | 0:056706c688ae | 89 | void sampleAccelerometer(void); |
aberk | 0:056706c688ae | 90 | //Set up the ITG3200 appropriately. |
aberk | 0:056706c688ae | 91 | void initializeGyroscope(void); |
aberk | 0:056706c688ae | 92 | //Calculate the null bias. |
aberk | 0:056706c688ae | 93 | void calibrateGyroscope(void); |
aberk | 0:056706c688ae | 94 | //Take a set of samples and average them. |
aberk | 0:056706c688ae | 95 | void sampleGyroscope(void); |
aberk | 0:056706c688ae | 96 | //Update the filter and calculate the Euler angles. |
aberk | 0:056706c688ae | 97 | void filter(void); |
aberk | 0:056706c688ae | 98 | |
aberk | 0:056706c688ae | 99 | void initializeAccelerometer(void) { |
aberk | 0:056706c688ae | 100 | |
aberk | 0:056706c688ae | 101 | //Go into standby mode to configure the device. |
aberk | 0:056706c688ae | 102 | accelerometer.setPowerControl(0x00); |
aberk | 0:056706c688ae | 103 | //Full resolution, +/-16g, 4mg/LSB. |
aberk | 0:056706c688ae | 104 | accelerometer.setDataFormatControl(0x0B); |
aberk | 0:056706c688ae | 105 | //200Hz data rate. |
aberk | 0:056706c688ae | 106 | accelerometer.setDataRate(ADXL345_200HZ); |
aberk | 0:056706c688ae | 107 | //Measurement mode. |
aberk | 0:056706c688ae | 108 | accelerometer.setPowerControl(0x08); |
aberk | 0:056706c688ae | 109 | //See http://www.analog.com/static/imported-files/application_notes/AN-1077.pdf |
aberk | 0:056706c688ae | 110 | wait_ms(22); |
aberk | 0:056706c688ae | 111 | |
aberk | 0:056706c688ae | 112 | } |
aberk | 0:056706c688ae | 113 | |
aberk | 0:056706c688ae | 114 | void sampleAccelerometer(void) { |
aberk | 0:056706c688ae | 115 | |
aberk | 0:056706c688ae | 116 | //Have we taken enough samples? |
aberk | 0:056706c688ae | 117 | if (accelerometerSamples == SAMPLES) { |
aberk | 0:056706c688ae | 118 | |
aberk | 0:056706c688ae | 119 | //Average the samples, remove the bias, and calculate the acceleration |
aberk | 0:056706c688ae | 120 | //in m/s/s. |
aberk | 0:056706c688ae | 121 | a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN; |
aberk | 0:056706c688ae | 122 | a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN; |
aberk | 0:056706c688ae | 123 | a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN; |
aberk | 0:056706c688ae | 124 | |
aberk | 0:056706c688ae | 125 | a_xAccumulator = 0; |
aberk | 0:056706c688ae | 126 | a_yAccumulator = 0; |
aberk | 0:056706c688ae | 127 | a_zAccumulator = 0; |
aberk | 0:056706c688ae | 128 | accelerometerSamples = 0; |
aberk | 0:056706c688ae | 129 | |
aberk | 0:056706c688ae | 130 | } else { |
aberk | 0:056706c688ae | 131 | //Take another sample. |
aberk | 0:056706c688ae | 132 | accelerometer.getOutput(readings); |
aberk | 0:056706c688ae | 133 | |
aberk | 0:056706c688ae | 134 | a_xAccumulator += (int16_t) readings[0]; |
aberk | 0:056706c688ae | 135 | a_yAccumulator += (int16_t) readings[1]; |
aberk | 0:056706c688ae | 136 | a_zAccumulator += (int16_t) readings[2]; |
aberk | 0:056706c688ae | 137 | |
aberk | 0:056706c688ae | 138 | accelerometerSamples++; |
aberk | 0:056706c688ae | 139 | |
aberk | 0:056706c688ae | 140 | } |
aberk | 0:056706c688ae | 141 | |
aberk | 0:056706c688ae | 142 | } |
aberk | 0:056706c688ae | 143 | |
aberk | 0:056706c688ae | 144 | void calibrateAccelerometer(void) { |
aberk | 0:056706c688ae | 145 | |
aberk | 0:056706c688ae | 146 | a_xAccumulator = 0; |
aberk | 0:056706c688ae | 147 | a_yAccumulator = 0; |
aberk | 0:056706c688ae | 148 | a_zAccumulator = 0; |
aberk | 0:056706c688ae | 149 | |
aberk | 0:056706c688ae | 150 | //Take a number of readings and average them |
aberk | 0:056706c688ae | 151 | //to calculate the zero g offset. |
aberk | 0:056706c688ae | 152 | for (int i = 0; i < CALIBRATION_SAMPLES; i++) { |
aberk | 0:056706c688ae | 153 | |
aberk | 0:056706c688ae | 154 | accelerometer.getOutput(readings); |
aberk | 0:056706c688ae | 155 | |
aberk | 0:056706c688ae | 156 | a_xAccumulator += (int16_t) readings[0]; |
aberk | 0:056706c688ae | 157 | a_yAccumulator += (int16_t) readings[1]; |
aberk | 0:056706c688ae | 158 | a_zAccumulator += (int16_t) readings[2]; |
aberk | 0:056706c688ae | 159 | |
aberk | 0:056706c688ae | 160 | wait(ACC_RATE); |
aberk | 0:056706c688ae | 161 | |
aberk | 0:056706c688ae | 162 | } |
aberk | 0:056706c688ae | 163 | |
aberk | 0:056706c688ae | 164 | a_xAccumulator /= CALIBRATION_SAMPLES; |
aberk | 0:056706c688ae | 165 | a_yAccumulator /= CALIBRATION_SAMPLES; |
aberk | 0:056706c688ae | 166 | a_zAccumulator /= CALIBRATION_SAMPLES; |
aberk | 0:056706c688ae | 167 | |
aberk | 0:056706c688ae | 168 | //At 4mg/LSB, 250 LSBs is 1g. |
aberk | 0:056706c688ae | 169 | a_xBias = a_xAccumulator; |
aberk | 0:056706c688ae | 170 | a_yBias = a_yAccumulator; |
aberk | 0:056706c688ae | 171 | a_zBias = (a_zAccumulator - 250); |
aberk | 0:056706c688ae | 172 | |
aberk | 0:056706c688ae | 173 | a_xAccumulator = 0; |
aberk | 0:056706c688ae | 174 | a_yAccumulator = 0; |
aberk | 0:056706c688ae | 175 | a_zAccumulator = 0; |
aberk | 0:056706c688ae | 176 | |
aberk | 0:056706c688ae | 177 | } |
aberk | 0:056706c688ae | 178 | |
aberk | 0:056706c688ae | 179 | void initializeGyroscope(void) { |
aberk | 0:056706c688ae | 180 | |
aberk | 0:056706c688ae | 181 | //Low pass filter bandwidth of 42Hz. |
aberk | 0:056706c688ae | 182 | gyroscope.setLpBandwidth(LPFBW_42HZ); |
aberk | 0:056706c688ae | 183 | //Internal sample rate of 200Hz. (1kHz / 5). |
aberk | 0:056706c688ae | 184 | gyroscope.setSampleRateDivider(4); |
aberk | 0:056706c688ae | 185 | |
aberk | 0:056706c688ae | 186 | } |
aberk | 0:056706c688ae | 187 | |
aberk | 0:056706c688ae | 188 | void calibrateGyroscope(void) { |
aberk | 0:056706c688ae | 189 | |
aberk | 0:056706c688ae | 190 | w_xAccumulator = 0; |
aberk | 0:056706c688ae | 191 | w_yAccumulator = 0; |
aberk | 0:056706c688ae | 192 | w_zAccumulator = 0; |
aberk | 0:056706c688ae | 193 | |
aberk | 0:056706c688ae | 194 | //Take a number of readings and average them |
aberk | 0:056706c688ae | 195 | //to calculate the gyroscope bias offset. |
aberk | 0:056706c688ae | 196 | for (int i = 0; i < CALIBRATION_SAMPLES; i++) { |
aberk | 0:056706c688ae | 197 | |
aberk | 0:056706c688ae | 198 | w_xAccumulator += gyroscope.getGyroX(); |
aberk | 0:056706c688ae | 199 | w_yAccumulator += gyroscope.getGyroY(); |
aberk | 0:056706c688ae | 200 | w_zAccumulator += gyroscope.getGyroZ(); |
aberk | 0:056706c688ae | 201 | wait(GYRO_RATE); |
aberk | 0:056706c688ae | 202 | |
aberk | 0:056706c688ae | 203 | } |
aberk | 0:056706c688ae | 204 | |
aberk | 0:056706c688ae | 205 | //Average the samples. |
aberk | 0:056706c688ae | 206 | w_xAccumulator /= CALIBRATION_SAMPLES; |
aberk | 0:056706c688ae | 207 | w_yAccumulator /= CALIBRATION_SAMPLES; |
aberk | 0:056706c688ae | 208 | w_zAccumulator /= CALIBRATION_SAMPLES; |
aberk | 0:056706c688ae | 209 | |
aberk | 0:056706c688ae | 210 | w_xBias = w_xAccumulator; |
aberk | 0:056706c688ae | 211 | w_yBias = w_yAccumulator; |
aberk | 0:056706c688ae | 212 | w_zBias = w_zAccumulator; |
aberk | 0:056706c688ae | 213 | |
aberk | 0:056706c688ae | 214 | w_xAccumulator = 0; |
aberk | 0:056706c688ae | 215 | w_yAccumulator = 0; |
aberk | 0:056706c688ae | 216 | w_zAccumulator = 0; |
aberk | 0:056706c688ae | 217 | |
aberk | 0:056706c688ae | 218 | } |
aberk | 0:056706c688ae | 219 | |
aberk | 0:056706c688ae | 220 | void sampleGyroscope(void) { |
aberk | 0:056706c688ae | 221 | |
aberk | 0:056706c688ae | 222 | //Have we taken enough samples? |
aberk | 0:056706c688ae | 223 | if (gyroscopeSamples == SAMPLES) { |
aberk | 0:056706c688ae | 224 | |
aberk | 0:056706c688ae | 225 | //Average the samples, remove the bias, and calculate the angular |
aberk | 0:056706c688ae | 226 | //velocity in rad/s. |
aberk | 0:056706c688ae | 227 | w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN); |
aberk | 0:056706c688ae | 228 | w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN); |
aberk | 0:056706c688ae | 229 | w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN); |
aberk | 0:056706c688ae | 230 | |
aberk | 0:056706c688ae | 231 | w_xAccumulator = 0; |
aberk | 0:056706c688ae | 232 | w_yAccumulator = 0; |
aberk | 0:056706c688ae | 233 | w_zAccumulator = 0; |
aberk | 0:056706c688ae | 234 | gyroscopeSamples = 0; |
aberk | 0:056706c688ae | 235 | |
aberk | 0:056706c688ae | 236 | } else { |
aberk | 0:056706c688ae | 237 | //Take another sample. |
aberk | 0:056706c688ae | 238 | w_xAccumulator += gyroscope.getGyroX(); |
aberk | 0:056706c688ae | 239 | w_yAccumulator += gyroscope.getGyroY(); |
aberk | 0:056706c688ae | 240 | w_zAccumulator += gyroscope.getGyroZ(); |
aberk | 0:056706c688ae | 241 | |
aberk | 0:056706c688ae | 242 | gyroscopeSamples++; |
aberk | 0:056706c688ae | 243 | |
aberk | 0:056706c688ae | 244 | } |
aberk | 0:056706c688ae | 245 | |
aberk | 0:056706c688ae | 246 | } |
aberk | 0:056706c688ae | 247 | |
aberk | 0:056706c688ae | 248 | void filter(void) { |
aberk | 0:056706c688ae | 249 | |
aberk | 0:056706c688ae | 250 | //Update the filter variables. |
aberk | 0:056706c688ae | 251 | imuFilter.updateFilter(w_y, w_x, w_z, a_y, a_x, a_z); |
aberk | 0:056706c688ae | 252 | //Calculate the new Euler angles. |
aberk | 0:056706c688ae | 253 | imuFilter.computeEuler(); |
aberk | 0:056706c688ae | 254 | |
aberk | 0:056706c688ae | 255 | } |
aberk | 0:056706c688ae | 256 | |
aberk | 0:056706c688ae | 257 | int main() { |
aberk | 0:056706c688ae | 258 | |
aberk | 0:056706c688ae | 259 | pc.printf("Starting IMU filter test...\n"); |
aberk | 0:056706c688ae | 260 | |
aberk | 0:056706c688ae | 261 | //Initialize inertial sensors. |
aberk | 0:056706c688ae | 262 | initializeAccelerometer(); |
aberk | 0:056706c688ae | 263 | calibrateAccelerometer(); |
aberk | 0:056706c688ae | 264 | initializeGyroscope(); |
aberk | 0:056706c688ae | 265 | calibrateGyroscope(); |
aberk | 0:056706c688ae | 266 | |
aberk | 0:056706c688ae | 267 | |
aberk | 0:056706c688ae | 268 | //Set up timers. |
aberk | 0:056706c688ae | 269 | //Accelerometer data rate is 200Hz, so we'll sample at this speed. |
aberk | 0:056706c688ae | 270 | accelerometerTicker.attach(&sampleAccelerometer, 0.005); |
aberk | 0:056706c688ae | 271 | //Gyroscope data rate is 200Hz, so we'll sample at this speed. |
aberk | 0:056706c688ae | 272 | gyroscopeTicker.attach(&sampleGyroscope, 0.005); |
aberk | 0:056706c688ae | 273 | //Update the filter variables at the correct rate. |
aberk | 0:056706c688ae | 274 | filterTicker.attach(&filter, FILTER_RATE); |
aberk | 0:056706c688ae | 275 | |
aberk | 0:056706c688ae | 276 | while (1) { |
aberk | 0:056706c688ae | 277 | |
aberk | 0:056706c688ae | 278 | wait(FILTER_RATE); |
aberk | 0:056706c688ae | 279 | |
aberk | 0:056706c688ae | 280 | |
aberk | 0:056706c688ae | 281 | pc.printf("%f,%f,%f\n",toDegrees(imuFilter.getRoll()), |
aberk | 0:056706c688ae | 282 | toDegrees(imuFilter.getPitch()), |
aberk | 0:056706c688ae | 283 | toDegrees(imuFilter.getYaw())); |
aberk | 0:056706c688ae | 284 | |
aberk | 0:056706c688ae | 285 | |
aberk | 0:056706c688ae | 286 | } |
aberk | 0:056706c688ae | 287 | |
aberk | 0:056706c688ae | 288 | } |