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

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 /**
00002  * IMU filter example.
00003  *
00004  * Calculate the roll, pitch and yaw angles.
00005  */
00006 #include "IMUfilter.h"
00007 #include "ADXL345.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 128
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 Serial pc(USBTX, USBRX);
00033 //At rest the gyroscope is centred around 0 and goes between about
00034 //-5 and 5 counts. As 1 degrees/sec is ~15 LSB, error is roughly
00035 //5/15 = 0.3 degrees/sec.
00036 IMUfilter imuFilter(FILTER_RATE, 0.3);
00037 ADXL345 accelerometer(p5, p6, p7, p8);
00038 ITG3200 gyroscope(p9, p10);
00039 Ticker accelerometerTicker;
00040 Ticker gyroscopeTicker;
00041 Ticker filterTicker;
00042 
00043 //Offsets for the gyroscope.
00044 //The readings we take when the gyroscope is stationary won't be 0, so we'll
00045 //average a set of readings we do get when the gyroscope is stationary and
00046 //take those away from subsequent readings to ensure the gyroscope is offset
00047 //or "biased" to 0.
00048 double w_xBias;
00049 double w_yBias;
00050 double w_zBias;
00051 
00052 //Offsets for the accelerometer.
00053 //Same as with the gyroscope.
00054 double a_xBias;
00055 double a_yBias;
00056 double a_zBias;
00057 
00058 //Accumulators used for oversampling and then averaging.
00059 volatile double a_xAccumulator = 0;
00060 volatile double a_yAccumulator = 0;
00061 volatile double a_zAccumulator = 0;
00062 volatile double w_xAccumulator = 0;
00063 volatile double w_yAccumulator = 0;
00064 volatile double w_zAccumulator = 0;
00065 
00066 //Accelerometer and gyroscope readings for x, y, z axes.
00067 volatile double a_x;
00068 volatile double a_y;
00069 volatile double a_z;
00070 volatile double w_x;
00071 volatile double w_y;
00072 volatile double w_z;
00073 
00074 //Buffer for accelerometer readings.
00075 int readings[3];
00076 //Number of accelerometer samples we're on.
00077 int accelerometerSamples = 0;
00078 //Number of gyroscope samples we're on.
00079 int gyroscopeSamples = 0;
00080 
00081 /**
00082  * Prototypes
00083  */
00084 //Set up the ADXL345 appropriately.
00085 void initializeAcceleromter(void);
00086 //Calculate the null bias.
00087 void calibrateAccelerometer(void);
00088 //Take a set of samples and average them.
00089 void sampleAccelerometer(void);
00090 //Set up the ITG3200 appropriately.
00091 void initializeGyroscope(void);
00092 //Calculate the null bias.
00093 void calibrateGyroscope(void);
00094 //Take a set of samples and average them.
00095 void sampleGyroscope(void);
00096 //Update the filter and calculate the Euler angles.
00097 void filter(void);
00098 
00099 void initializeAccelerometer(void) {
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     //Have we taken enough samples?
00117     if (accelerometerSamples == SAMPLES) {
00118 
00119         //Average the samples, remove the bias, and calculate the acceleration
00120         //in m/s/s.
00121         a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN;
00122         a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN;
00123         a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN;
00124 
00125         a_xAccumulator = 0;
00126         a_yAccumulator = 0;
00127         a_zAccumulator = 0;
00128         accelerometerSamples = 0;
00129 
00130     } else {
00131         //Take another sample.
00132         accelerometer.getOutput(readings);
00133 
00134         a_xAccumulator += (int16_t) readings[0];
00135         a_yAccumulator += (int16_t) readings[1];
00136         a_zAccumulator += (int16_t) readings[2];
00137 
00138         accelerometerSamples++;
00139 
00140     }
00141 
00142 }
00143 
00144 void calibrateAccelerometer(void) {
00145 
00146     a_xAccumulator = 0;
00147     a_yAccumulator = 0;
00148     a_zAccumulator = 0;
00149     
00150     //Take a number of readings and average them
00151     //to calculate the zero g offset.
00152     for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
00153 
00154         accelerometer.getOutput(readings);
00155 
00156         a_xAccumulator += (int16_t) readings[0];
00157         a_yAccumulator += (int16_t) readings[1];
00158         a_zAccumulator += (int16_t) readings[2];
00159 
00160         wait(ACC_RATE);
00161 
00162     }
00163 
00164     a_xAccumulator /= CALIBRATION_SAMPLES;
00165     a_yAccumulator /= CALIBRATION_SAMPLES;
00166     a_zAccumulator /= CALIBRATION_SAMPLES;
00167 
00168     //At 4mg/LSB, 250 LSBs is 1g.
00169     a_xBias = a_xAccumulator;
00170     a_yBias = a_yAccumulator;
00171     a_zBias = (a_zAccumulator - 250);
00172 
00173     a_xAccumulator = 0;
00174     a_yAccumulator = 0;
00175     a_zAccumulator = 0;
00176 
00177 }
00178 
00179 void initializeGyroscope(void) {
00180 
00181     //Low pass filter bandwidth of 42Hz.
00182     gyroscope.setLpBandwidth(LPFBW_42HZ);
00183     //Internal sample rate of 200Hz. (1kHz / 5).
00184     gyroscope.setSampleRateDivider(4);
00185 
00186 }
00187 
00188 void calibrateGyroscope(void) {
00189 
00190     w_xAccumulator = 0;
00191     w_yAccumulator = 0;
00192     w_zAccumulator = 0;
00193 
00194     //Take a number of readings and average them
00195     //to calculate the gyroscope bias offset.
00196     for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
00197 
00198         w_xAccumulator += gyroscope.getGyroX();
00199         w_yAccumulator += gyroscope.getGyroY();
00200         w_zAccumulator += gyroscope.getGyroZ();
00201         wait(GYRO_RATE);
00202 
00203     }
00204 
00205     //Average the samples.
00206     w_xAccumulator /= CALIBRATION_SAMPLES;
00207     w_yAccumulator /= CALIBRATION_SAMPLES;
00208     w_zAccumulator /= CALIBRATION_SAMPLES;
00209 
00210     w_xBias = w_xAccumulator;
00211     w_yBias = w_yAccumulator;
00212     w_zBias = w_zAccumulator;
00213 
00214     w_xAccumulator = 0;
00215     w_yAccumulator = 0;
00216     w_zAccumulator = 0;
00217 
00218 }
00219 
00220 void sampleGyroscope(void) {
00221 
00222     //Have we taken enough samples?
00223     if (gyroscopeSamples == SAMPLES) {
00224 
00225         //Average the samples, remove the bias, and calculate the angular
00226         //velocity in rad/s.
00227         w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN);
00228         w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN);
00229         w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN);
00230 
00231         w_xAccumulator = 0;
00232         w_yAccumulator = 0;
00233         w_zAccumulator = 0;
00234         gyroscopeSamples = 0;
00235 
00236     } else {
00237         //Take another sample.
00238         w_xAccumulator += gyroscope.getGyroX();
00239         w_yAccumulator += gyroscope.getGyroY();
00240         w_zAccumulator += gyroscope.getGyroZ();
00241 
00242         gyroscopeSamples++;
00243 
00244     }
00245 
00246 }
00247 
00248 void filter(void) {
00249 
00250     //Update the filter variables.
00251     imuFilter.updateFilter(w_y, w_x, w_z, a_y, a_x, a_z);
00252     //Calculate the new Euler angles.
00253     imuFilter.computeEuler();
00254 
00255 }
00256 
00257 int main() {
00258 
00259     pc.printf("Starting IMU filter test...\n");
00260 
00261     //Initialize inertial sensors.
00262     initializeAccelerometer();
00263     calibrateAccelerometer();
00264     initializeGyroscope();
00265     calibrateGyroscope();
00266 
00267 
00268     //Set up timers.
00269     //Accelerometer data rate is 200Hz, so we'll sample at this speed.
00270     accelerometerTicker.attach(&sampleAccelerometer, 0.005);
00271     //Gyroscope data rate is 200Hz, so we'll sample at this speed.
00272     gyroscopeTicker.attach(&sampleGyroscope, 0.005);
00273     //Update the filter variables at the correct rate.
00274     filterTicker.attach(&filter, FILTER_RATE);
00275    
00276     while (1) {
00277 
00278         wait(FILTER_RATE);
00279 
00280         
00281         pc.printf("%f,%f,%f\n",toDegrees(imuFilter.getRoll()),
00282                   toDegrees(imuFilter.getPitch()),
00283                   toDegrees(imuFilter.getYaw()));
00284          
00285 
00286     }
00287 
00288 }