Justin Eng / Mbed 2 deprecated SuperMbedBall

This is a ball on the Mbed.

Dependencies:   4DGL-uLCD-SE IMUfilter ITG3200 Music mbed-rtos mbed

IMU_RPY.h@0:680348a938f8, 2014-03-14 (annotated)

Committer:
Strikewolf
Date:
Fri Mar 14 17:03:16 2014 +0000
Revision:
0:680348a938f8
Initial commit;

Who changed what in which revision?

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