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
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 }
Generated on Wed Jul 13 2022 07:48:06 by 1.7.2