ohararp / Mbed 2 deprecated IMUFilter_9DOF-Stick_RY

Dependencies:   mbed

main.cpp@0:d536780893d9, 2012-06-08 (annotated)

Committer:
ohararp
Date:
Fri Jun 08 13:42:48 2012 +0000
Revision:
0:d536780893d9
Example Code modified to run with Sparkfun 9DOF stick

Who changed what in which revision?

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