IMU_RPY.h

00001 /**
00002  * IMU filter example.
00003  *
00004  * Calculate the roll, pitch and yaw angles.
00005  */
00006 #include "IMUfilter.h"
00007 #include "ADXL345_I2C.h"
00008 #include "ITG3200.h"
00009 
00010 //Gravity at Earth's surface in m/s/s
00011 #define g0 9.812865328
00012 //Number of samples to average.
00013 #define SAMPLES 4
00014 //Number of samples to be averaged for a null bias calculation
00015 //during calibration.
00016 #define CALIBRATION_SAMPLES 32
00017 //Convert from radians to degrees.
00018 #define toDegrees(x) (x * 57.2957795)
00019 //Convert from degrees to radians.
00020 #define toRadians(x) (x * 0.01745329252)
00021 //ITG-3200 sensitivity is 14.375 LSB/(degrees/sec).
00022 #define GYROSCOPE_GAIN (1 / 14.375)
00023 //Full scale resolution on the ADXL345 is 4mg/LSB.
00024 #define ACCELEROMETER_GAIN (0.004 * g0)
00025 //Sampling gyroscope at 200Hz.
00026 #define GYRO_RATE   0.005
00027 //Sampling accelerometer at 200Hz.
00028 #define ACC_RATE    0.005
00029 //Updating filter at 40Hz.
00030 #define FILTER_RATE 0.1
00031 
00032 //At rest the gyroscope is centred around 0 and goes between about
00033 //-5 and 5 counts. As 1 degrees/sec is ~15 LSB, error is roughly
00034 //5/15 = 0.3 degrees/sec.
00035 IMUfilter imuFilter(FILTER_RATE, 0.3);
00036 ADXL345_I2C accelerometer(p9, p10);
00037 ITG3200 gyroscope(p9, p10);
00038 Ticker accelerometerTicker;
00039 Ticker gyroscopeTicker;
00040 Ticker filterTicker;
00041 
00042 //Offsets for the gyroscope.
00043 //The readings we take when the gyroscope is stationary won't be 0, so we'll
00044 //average a set of readings we do get when the gyroscope is stationary and
00045 //take those away from subsequent readings to ensure the gyroscope is offset
00046 //or "biased" to 0.
00047 double w_xBias;
00048 double w_yBias;
00049 double w_zBias;
00050 
00051 //Offsets for the accelerometer.
00052 //Same as with the gyroscope.
00053 double a_xBias;
00054 double a_yBias;
00055 double a_zBias;
00056 
00057 //Accumulators used for oversampling and then averaging.
00058 volatile double a_xAccumulator = 0;
00059 volatile double a_yAccumulator = 0;
00060 volatile double a_zAccumulator = 0;
00061 volatile double w_xAccumulator = 0;
00062 volatile double w_yAccumulator = 0;
00063 volatile double w_zAccumulator = 0;
00064 
00065 //Accelerometer and gyroscope readings for x, y, z axes.
00066 volatile double a_x;
00067 volatile double a_y;
00068 volatile double a_z;
00069 volatile double w_x;
00070 volatile double w_y;
00071 volatile double w_z;
00072 
00073 //Buffer for accelerometer readings.
00074 int readings[3];
00075 //Number of accelerometer samples we're on.
00076 int accelerometerSamples = 0;
00077 //Number of gyroscope samples we're on.
00078 int gyroscopeSamples = 0;
00079 
00080 /**
00081  * Prototypes
00082  */
00083 //Set up the ADXL345 appropriately.
00084 void initializeAcceleromter(void);
00085 //Calculate the null bias.
00086 void calibrateAccelerometer(void);
00087 //Take a set of samples and average them.
00088 void sampleAccelerometer(void);
00089 //Set up the ITG3200 appropriately.
00090 void initializeGyroscope(void);
00091 //Calculate the null bias.
00092 void calibrateGyroscope(void);
00093 //Take a set of samples and average them.
00094 void sampleGyroscope(void);
00095 //Update the filter and calculate the Euler angles.
00096 void filter(void);
00097 
00098 void initializeAccelerometer(void)
00099 {
00100 
00101     //Go into standby mode to configure the device.
00102     accelerometer.setPowerControl(0x00);
00103     //Full resolution, +/-16g, 4mg/LSB.
00104     accelerometer.setDataFormatControl(0x0B);
00105     //200Hz data rate.
00106     accelerometer.setDataRate(ADXL345_200HZ);
00107     //Measurement mode.
00108     accelerometer.setPowerControl(0x08);
00109     //See http://www.analog.com/static/imported-files/application_notes/AN-1077.pdf
00110     wait_ms(22);
00111 
00112 }
00113 
00114 void sampleAccelerometer(void)
00115 {
00116 
00117     //Have we taken enough samples?
00118     if (accelerometerSamples == SAMPLES) {
00119 
00120         //Average the samples, remove the bias, and calculate the acceleration
00121         //in m/s/s.
00122         a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN;
00123         a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN;
00124         a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN;
00125 
00126         a_xAccumulator = 0;
00127         a_yAccumulator = 0;
00128         a_zAccumulator = 0;
00129         accelerometerSamples = 0;
00130 
00131     } else {
00132         //Take another sample.
00133         accelerometer.getOutput(readings);
00134 
00135         a_xAccumulator += (int16_t) readings[0];
00136         a_yAccumulator += (int16_t) readings[1];
00137         a_zAccumulator += (int16_t) readings[2];
00138 
00139         accelerometerSamples++;
00140 
00141     }
00142 
00143 }
00144 
00145 void calibrateAccelerometer(void)
00146 {
00147 
00148     a_xAccumulator = 0;
00149     a_yAccumulator = 0;
00150     a_zAccumulator = 0;
00151 
00152     //Take a number of readings and average them
00153     //to calculate the zero g offset.
00154     for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
00155 
00156         accelerometer.getOutput(readings);
00157 
00158         a_xAccumulator += (int16_t) readings[0];
00159         a_yAccumulator += (int16_t) readings[1];
00160         a_zAccumulator += (int16_t) readings[2];
00161 
00162         wait(ACC_RATE);
00163 
00164     }
00165 
00166     a_xAccumulator /= CALIBRATION_SAMPLES;
00167     a_yAccumulator /= CALIBRATION_SAMPLES;
00168     a_zAccumulator /= CALIBRATION_SAMPLES;
00169 
00170     //At 4mg/LSB, 250 LSBs is 1g.
00171     a_xBias = a_xAccumulator;
00172     a_yBias = a_yAccumulator;
00173     a_zBias = (a_zAccumulator - 250);
00174 
00175     a_xAccumulator = 0;
00176     a_yAccumulator = 0;
00177     a_zAccumulator = 0;
00178 
00179 }
00180 
00181 void initializeGyroscope(void)
00182 {
00183 
00184     //Low pass filter bandwidth of 42Hz.
00185     gyroscope.setLpBandwidth(LPFBW_42HZ);
00186     //Internal sample rate of 200Hz. (1kHz / 5).
00187     gyroscope.setSampleRateDivider(4);
00188 
00189 }
00190 
00191 void calibrateGyroscope(void)
00192 {
00193 
00194     w_xAccumulator = 0;
00195     w_yAccumulator = 0;
00196     w_zAccumulator = 0;
00197 
00198     //Take a number of readings and average them
00199     //to calculate the gyroscope bias offset.
00200     for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
00201 
00202         w_xAccumulator += gyroscope.getGyroX();
00203         w_yAccumulator += gyroscope.getGyroY();
00204         w_zAccumulator += gyroscope.getGyroZ();
00205         wait(GYRO_RATE);
00206 
00207     }
00208 
00209     //Average the samples.
00210     w_xAccumulator /= CALIBRATION_SAMPLES;
00211     w_yAccumulator /= CALIBRATION_SAMPLES;
00212     w_zAccumulator /= CALIBRATION_SAMPLES;
00213 
00214     w_xBias = w_xAccumulator;
00215     w_yBias = w_yAccumulator;
00216     w_zBias = w_zAccumulator;
00217 
00218     w_xAccumulator = 0;
00219     w_yAccumulator = 0;
00220     w_zAccumulator = 0;
00221 
00222 }
00223 
00224 void sampleGyroscope(void)
00225 {
00226 
00227     //Have we taken enough samples?
00228     if (gyroscopeSamples == SAMPLES) {
00229 
00230         //Average the samples, remove the bias, and calculate the angular
00231         //velocity in rad/s.
00232         w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN);
00233         w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN);
00234         w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN);
00235 
00236         w_xAccumulator = 0;
00237         w_yAccumulator = 0;
00238         w_zAccumulator = 0;
00239         gyroscopeSamples = 0;
00240 
00241     } else {
00242         //Take another sample.
00243         w_xAccumulator += gyroscope.getGyroX();
00244         w_yAccumulator += gyroscope.getGyroY();
00245         w_zAccumulator += gyroscope.getGyroZ();
00246 
00247         gyroscopeSamples++;
00248 
00249     }
00250 
00251 }
00252 
00253 void filter(void)
00254 {
00255 
00256     //Update the filter variables.
00257     imuFilter.updateFilter(w_y, w_x, w_z, a_y, a_x, a_z);
00258     //Calculate the new Euler angles.
00259     imuFilter.computeEuler();
00260 
00261 }
00262 
00263 void rpy_init()
00264 {
00265     //Initialize inertial sensors.
00266     initializeAccelerometer();
00267     calibrateAccelerometer();
00268     initializeGyroscope();
00269     calibrateGyroscope();
00270 
00271 
00272     //Set up timers.
00273     //Accelerometer data rate is 200Hz, so we'll sample at this speed.
00274     accelerometerTicker.attach(&sampleAccelerometer, 0.005);
00275     //Gyroscope data rate is 200Hz, so we'll sample at this speed.
00276     gyroscopeTicker.attach(&sampleGyroscope, 0.005);
00277     //Update the filter variables at the correct rate.
00278     filterTicker.attach(&filter, FILTER_RATE);
00279 }