The IMUfilter Roll/Pitch/Yaw example wrapped up into a class for easier use with other programs: http://mbed.org/users/aberk/programs/IMUfilter_RPYExample/latest
IMU.h@0:51d01aa47c71, 2010-08-11 (annotated)
- Committer:
- aberk
- Date:
- Wed Aug 11 11:09:47 2010 +0000
- Revision:
- 0:51d01aa47c71
Version 1.0
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
aberk | 0:51d01aa47c71 | 1 | /** |
aberk | 0:51d01aa47c71 | 2 | * @section LICENSE |
aberk | 0:51d01aa47c71 | 3 | * |
aberk | 0:51d01aa47c71 | 4 | * Copyright (c) 2010 ARM Limited |
aberk | 0:51d01aa47c71 | 5 | * |
aberk | 0:51d01aa47c71 | 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy |
aberk | 0:51d01aa47c71 | 7 | * of this software and associated documentation files (the "Software"), to deal |
aberk | 0:51d01aa47c71 | 8 | * in the Software without restriction, including without limitation the rights |
aberk | 0:51d01aa47c71 | 9 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell |
aberk | 0:51d01aa47c71 | 10 | * copies of the Software, and to permit persons to whom the Software is |
aberk | 0:51d01aa47c71 | 11 | * furnished to do so, subject to the following conditions: |
aberk | 0:51d01aa47c71 | 12 | * |
aberk | 0:51d01aa47c71 | 13 | * The above copyright notice and this permission notice shall be included in |
aberk | 0:51d01aa47c71 | 14 | * all copies or substantial portions of the Software. |
aberk | 0:51d01aa47c71 | 15 | * |
aberk | 0:51d01aa47c71 | 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR |
aberk | 0:51d01aa47c71 | 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, |
aberk | 0:51d01aa47c71 | 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE |
aberk | 0:51d01aa47c71 | 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER |
aberk | 0:51d01aa47c71 | 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
aberk | 0:51d01aa47c71 | 21 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN |
aberk | 0:51d01aa47c71 | 22 | * THE SOFTWARE. |
aberk | 0:51d01aa47c71 | 23 | * |
aberk | 0:51d01aa47c71 | 24 | * @section DESCRIPTION |
aberk | 0:51d01aa47c71 | 25 | * |
aberk | 0:51d01aa47c71 | 26 | * IMU consisting of ADXL345 accelerometer and ITG-3200 gyroscope using |
aberk | 0:51d01aa47c71 | 27 | * orientation filter developed by Sebastian Madgwick. |
aberk | 0:51d01aa47c71 | 28 | * |
aberk | 0:51d01aa47c71 | 29 | * Find more details about his paper here: |
aberk | 0:51d01aa47c71 | 30 | * |
aberk | 0:51d01aa47c71 | 31 | * http://code.google.com/p/imumargalgorithm30042010sohm/ |
aberk | 0:51d01aa47c71 | 32 | */ |
aberk | 0:51d01aa47c71 | 33 | |
aberk | 0:51d01aa47c71 | 34 | #ifndef MBED_IMU_H |
aberk | 0:51d01aa47c71 | 35 | #define MBED_IMU_H |
aberk | 0:51d01aa47c71 | 36 | |
aberk | 0:51d01aa47c71 | 37 | /** |
aberk | 0:51d01aa47c71 | 38 | * Includes |
aberk | 0:51d01aa47c71 | 39 | */ |
aberk | 0:51d01aa47c71 | 40 | #include "mbed.h" |
aberk | 0:51d01aa47c71 | 41 | #include "ADXL345.h" |
aberk | 0:51d01aa47c71 | 42 | #include "ITG3200.h" |
aberk | 0:51d01aa47c71 | 43 | #include "IMUfilter.h" |
aberk | 0:51d01aa47c71 | 44 | |
aberk | 0:51d01aa47c71 | 45 | /** |
aberk | 0:51d01aa47c71 | 46 | * Defines |
aberk | 0:51d01aa47c71 | 47 | */ |
aberk | 0:51d01aa47c71 | 48 | #define IMU_RATE 0.025 |
aberk | 0:51d01aa47c71 | 49 | #define ACCELEROMETER_RATE 0.005 |
aberk | 0:51d01aa47c71 | 50 | #define GYROSCOPE_RATE 0.005 |
aberk | 0:51d01aa47c71 | 51 | #define GYRO_MEAS_ERROR 0.3 //IMUfilter tuning parameter. |
aberk | 0:51d01aa47c71 | 52 | |
aberk | 0:51d01aa47c71 | 53 | //Gravity at Earth's surface in m/s/s |
aberk | 0:51d01aa47c71 | 54 | #define g0 9.812865328 |
aberk | 0:51d01aa47c71 | 55 | //Number of samples to average |
aberk | 0:51d01aa47c71 | 56 | #define SAMPLES 4 |
aberk | 0:51d01aa47c71 | 57 | #define CALIBRATION_SAMPLES 128 |
aberk | 0:51d01aa47c71 | 58 | //Multiply radians to get degrees. |
aberk | 0:51d01aa47c71 | 59 | #define toDegrees(x) (x * 57.2957795) |
aberk | 0:51d01aa47c71 | 60 | //Multiply degrees to get radians. |
aberk | 0:51d01aa47c71 | 61 | #define toRadians(x) (x * 0.01745329252) |
aberk | 0:51d01aa47c71 | 62 | //Full scale resolution on the ADXL345 is 4mg/LSB. |
aberk | 0:51d01aa47c71 | 63 | //Multiply ADC count readings from ADXL345 to get acceleration in m/s/s. |
aberk | 0:51d01aa47c71 | 64 | #define toAcceleration(x) (x * (4 * g0 * 0.001)) |
aberk | 0:51d01aa47c71 | 65 | //14.375 LSB/(degrees/sec) |
aberk | 0:51d01aa47c71 | 66 | #define GYROSCOPE_GAIN (1 / 14.375) |
aberk | 0:51d01aa47c71 | 67 | #define ACCELEROMETER_GAIN (0.004 * g0) |
aberk | 0:51d01aa47c71 | 68 | |
aberk | 0:51d01aa47c71 | 69 | /** |
aberk | 0:51d01aa47c71 | 70 | * IMU consisting of ADXL345 accelerometer and ITG-3200 gyroscope to calculate |
aberk | 0:51d01aa47c71 | 71 | * roll, pitch and yaw angles. |
aberk | 0:51d01aa47c71 | 72 | */ |
aberk | 0:51d01aa47c71 | 73 | class IMU { |
aberk | 0:51d01aa47c71 | 74 | |
aberk | 0:51d01aa47c71 | 75 | public: |
aberk | 0:51d01aa47c71 | 76 | |
aberk | 0:51d01aa47c71 | 77 | /** |
aberk | 0:51d01aa47c71 | 78 | * Constructor. |
aberk | 0:51d01aa47c71 | 79 | * |
aberk | 0:51d01aa47c71 | 80 | * @param imuRate Rate which IMUfilter update and Euler angle calculation |
aberk | 0:51d01aa47c71 | 81 | * occurs. |
aberk | 0:51d01aa47c71 | 82 | * @param gyroscopeMeasurementError IMUfilter tuning parameter. |
aberk | 0:51d01aa47c71 | 83 | * @param accelerometerRate Rate at which accelerometer data is sampled. |
aberk | 0:51d01aa47c71 | 84 | * @param gyroscopeRate Rate at which gyroscope data is sampled. |
aberk | 0:51d01aa47c71 | 85 | */ |
aberk | 0:51d01aa47c71 | 86 | IMU(float imuRate, |
aberk | 0:51d01aa47c71 | 87 | double gyroscopeMeasurementError, |
aberk | 0:51d01aa47c71 | 88 | float accelerometerRate, |
aberk | 0:51d01aa47c71 | 89 | float gyroscopeRate); |
aberk | 0:51d01aa47c71 | 90 | |
aberk | 0:51d01aa47c71 | 91 | /** |
aberk | 0:51d01aa47c71 | 92 | * Get the current roll angle. |
aberk | 0:51d01aa47c71 | 93 | * |
aberk | 0:51d01aa47c71 | 94 | * @return The current roll angle in degrees. |
aberk | 0:51d01aa47c71 | 95 | */ |
aberk | 0:51d01aa47c71 | 96 | double getRoll(void); |
aberk | 0:51d01aa47c71 | 97 | |
aberk | 0:51d01aa47c71 | 98 | /** |
aberk | 0:51d01aa47c71 | 99 | * Get the current pitch angle. |
aberk | 0:51d01aa47c71 | 100 | * |
aberk | 0:51d01aa47c71 | 101 | * @return The current pitch angle in degrees. |
aberk | 0:51d01aa47c71 | 102 | */ |
aberk | 0:51d01aa47c71 | 103 | double getPitch(void); |
aberk | 0:51d01aa47c71 | 104 | |
aberk | 0:51d01aa47c71 | 105 | /** |
aberk | 0:51d01aa47c71 | 106 | * Get the current yaw angle. |
aberk | 0:51d01aa47c71 | 107 | * |
aberk | 0:51d01aa47c71 | 108 | * @return The current yaw angle in degrees. |
aberk | 0:51d01aa47c71 | 109 | */ |
aberk | 0:51d01aa47c71 | 110 | double getYaw(void); |
aberk | 0:51d01aa47c71 | 111 | |
aberk | 0:51d01aa47c71 | 112 | private: |
aberk | 0:51d01aa47c71 | 113 | |
aberk | 0:51d01aa47c71 | 114 | /** |
aberk | 0:51d01aa47c71 | 115 | * Set up the ADXL345 appropriately. |
aberk | 0:51d01aa47c71 | 116 | */ |
aberk | 0:51d01aa47c71 | 117 | void initializeAccelerometer(void); |
aberk | 0:51d01aa47c71 | 118 | |
aberk | 0:51d01aa47c71 | 119 | /** |
aberk | 0:51d01aa47c71 | 120 | * Calculate the zero g offset. |
aberk | 0:51d01aa47c71 | 121 | */ |
aberk | 0:51d01aa47c71 | 122 | void calibrateAccelerometer(void); |
aberk | 0:51d01aa47c71 | 123 | |
aberk | 0:51d01aa47c71 | 124 | /** |
aberk | 0:51d01aa47c71 | 125 | * Take a set of samples and average them. |
aberk | 0:51d01aa47c71 | 126 | */ |
aberk | 0:51d01aa47c71 | 127 | void sampleAccelerometer(void); |
aberk | 0:51d01aa47c71 | 128 | |
aberk | 0:51d01aa47c71 | 129 | /** |
aberk | 0:51d01aa47c71 | 130 | * Set up the ITG-3200 appropriately. |
aberk | 0:51d01aa47c71 | 131 | */ |
aberk | 0:51d01aa47c71 | 132 | void initializeGyroscope(void); |
aberk | 0:51d01aa47c71 | 133 | |
aberk | 0:51d01aa47c71 | 134 | /** |
aberk | 0:51d01aa47c71 | 135 | * Calculate the bias offset. |
aberk | 0:51d01aa47c71 | 136 | */ |
aberk | 0:51d01aa47c71 | 137 | void calibrateGyroscope(void); |
aberk | 0:51d01aa47c71 | 138 | |
aberk | 0:51d01aa47c71 | 139 | /** |
aberk | 0:51d01aa47c71 | 140 | * Take a set of samples and average them. |
aberk | 0:51d01aa47c71 | 141 | */ |
aberk | 0:51d01aa47c71 | 142 | void sampleGyroscope(void); |
aberk | 0:51d01aa47c71 | 143 | |
aberk | 0:51d01aa47c71 | 144 | /** |
aberk | 0:51d01aa47c71 | 145 | * Update the filter and calculate the Euler angles. |
aberk | 0:51d01aa47c71 | 146 | */ |
aberk | 0:51d01aa47c71 | 147 | void filter(void); |
aberk | 0:51d01aa47c71 | 148 | |
aberk | 0:51d01aa47c71 | 149 | ADXL345 accelerometer; |
aberk | 0:51d01aa47c71 | 150 | ITG3200 gyroscope; |
aberk | 0:51d01aa47c71 | 151 | IMUfilter imuFilter; |
aberk | 0:51d01aa47c71 | 152 | |
aberk | 0:51d01aa47c71 | 153 | Ticker accelerometerTicker; |
aberk | 0:51d01aa47c71 | 154 | Ticker gyroscopeTicker; |
aberk | 0:51d01aa47c71 | 155 | Ticker filterTicker; |
aberk | 0:51d01aa47c71 | 156 | |
aberk | 0:51d01aa47c71 | 157 | float accelerometerRate_; |
aberk | 0:51d01aa47c71 | 158 | float gyroscopeRate_; |
aberk | 0:51d01aa47c71 | 159 | float imuRate_; |
aberk | 0:51d01aa47c71 | 160 | |
aberk | 0:51d01aa47c71 | 161 | //Offsets for the gyroscope. |
aberk | 0:51d01aa47c71 | 162 | //The readings we take when the gyroscope is stationary won't be 0, so we'll |
aberk | 0:51d01aa47c71 | 163 | //average a set of readings we do get when the gyroscope is stationary and |
aberk | 0:51d01aa47c71 | 164 | //take those away from subsequent readings to ensure the gyroscope is offset |
aberk | 0:51d01aa47c71 | 165 | //or biased to 0. |
aberk | 0:51d01aa47c71 | 166 | double w_xBias; |
aberk | 0:51d01aa47c71 | 167 | double w_yBias; |
aberk | 0:51d01aa47c71 | 168 | double w_zBias; |
aberk | 0:51d01aa47c71 | 169 | |
aberk | 0:51d01aa47c71 | 170 | double a_xBias; |
aberk | 0:51d01aa47c71 | 171 | double a_yBias; |
aberk | 0:51d01aa47c71 | 172 | double a_zBias; |
aberk | 0:51d01aa47c71 | 173 | |
aberk | 0:51d01aa47c71 | 174 | volatile double a_xAccumulator; |
aberk | 0:51d01aa47c71 | 175 | volatile double a_yAccumulator; |
aberk | 0:51d01aa47c71 | 176 | volatile double a_zAccumulator; |
aberk | 0:51d01aa47c71 | 177 | volatile double w_xAccumulator; |
aberk | 0:51d01aa47c71 | 178 | volatile double w_yAccumulator; |
aberk | 0:51d01aa47c71 | 179 | volatile double w_zAccumulator; |
aberk | 0:51d01aa47c71 | 180 | |
aberk | 0:51d01aa47c71 | 181 | //Accelerometer and gyroscope readings for x, y, z axes. |
aberk | 0:51d01aa47c71 | 182 | volatile double a_x; |
aberk | 0:51d01aa47c71 | 183 | volatile double a_y; |
aberk | 0:51d01aa47c71 | 184 | volatile double a_z; |
aberk | 0:51d01aa47c71 | 185 | volatile double w_x; |
aberk | 0:51d01aa47c71 | 186 | volatile double w_y; |
aberk | 0:51d01aa47c71 | 187 | volatile double w_z; |
aberk | 0:51d01aa47c71 | 188 | |
aberk | 0:51d01aa47c71 | 189 | //Buffer for accelerometer readings. |
aberk | 0:51d01aa47c71 | 190 | int readings[3]; |
aberk | 0:51d01aa47c71 | 191 | int accelerometerSamples; |
aberk | 0:51d01aa47c71 | 192 | int gyroscopeSamples; |
aberk | 0:51d01aa47c71 | 193 | |
aberk | 0:51d01aa47c71 | 194 | }; |
aberk | 0:51d01aa47c71 | 195 | |
aberk | 0:51d01aa47c71 | 196 | #endif /* MBED_IMU_H */ |