Tim Marvin / Mbed 2 deprecated IMU

IMU code. Relies on LIS331_lib, modified ITG3200_lib, and IMUfilter_lib

Dependencies:   mbed ITG3200_lib

main.cpp@0:a070fa765ed2, 2010-11-18 (annotated)

Committer:
atommota
Date:
Thu Nov 18 01:19:16 2010 +0000
Revision:
0:a070fa765ed2
Initial attempt at an IMU code.

Who changed what in which revision?

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