IMU_RPY.h

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