Inertial measurement unit orientation filter. Ported from Sebastian Madgwick\'s paper, \"An efficient orientation filter for inertial and inertial/magnetic sensor arrays\".

Dependencies:   mbed ITG3200 ADXL345 IMUfilter

Committer:
aberk
Date:
Tue Aug 10 13:19:17 2010 +0000
Revision:
0:056706c688ae
Child:
1:dda873ab579f
Version 1.0

Who changed what in which revision?

UserRevisionLine numberNew 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 0:056706c688ae 30 #define FILTER_RATE 0.025
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 }