Spring 2014, ECE 4180 project, Georgia Institute of Technolgoy. This is the autonomous driver program for the Robotics Cat and Mouse program.
Dependencies: IMUfilter ADXL345_I2C mbed ITG3200 USBHost mbed-rtos
IMU_RPY.h@0:84d5aa80fd77, 2014-04-27 (annotated)
- Committer:
- Strikewolf
- Date:
- Sun Apr 27 04:31:07 2014 +0000
- Revision:
- 0:84d5aa80fd77
- Child:
- 1:dacf7db790f6
-v1.0 of human controlled car;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Strikewolf | 0:84d5aa80fd77 | 1 | |
Strikewolf | 0:84d5aa80fd77 | 2 | |
Strikewolf | 0:84d5aa80fd77 | 3 | /** |
Strikewolf | 0:84d5aa80fd77 | 4 | * IMU filter example. |
Strikewolf | 0:84d5aa80fd77 | 5 | * |
Strikewolf | 0:84d5aa80fd77 | 6 | * Calculate the roll, pitch and yaw angles. |
Strikewolf | 0:84d5aa80fd77 | 7 | */ |
Strikewolf | 0:84d5aa80fd77 | 8 | #include "IMUfilter.h" |
Strikewolf | 0:84d5aa80fd77 | 9 | #include "ADXL345_I2C.h" |
Strikewolf | 0:84d5aa80fd77 | 10 | #include "ITG3200.h" |
Strikewolf | 0:84d5aa80fd77 | 11 | |
Strikewolf | 0:84d5aa80fd77 | 12 | //Gravity at Earth's surface in m/s/s |
Strikewolf | 0:84d5aa80fd77 | 13 | #define g0 9.812865328 |
Strikewolf | 0:84d5aa80fd77 | 14 | //Number of samples to average. |
Strikewolf | 0:84d5aa80fd77 | 15 | #define SAMPLES 4 |
Strikewolf | 0:84d5aa80fd77 | 16 | //Number of samples to be averaged for a null bias calculation |
Strikewolf | 0:84d5aa80fd77 | 17 | //during calibration. |
Strikewolf | 0:84d5aa80fd77 | 18 | #define CALIBRATION_SAMPLES 128 |
Strikewolf | 0:84d5aa80fd77 | 19 | //Convert from radians to degrees. |
Strikewolf | 0:84d5aa80fd77 | 20 | #define toDegrees(x) (x * 57.2957795) |
Strikewolf | 0:84d5aa80fd77 | 21 | //Convert from degrees to radians. |
Strikewolf | 0:84d5aa80fd77 | 22 | #define toRadians(x) (x * 0.01745329252) |
Strikewolf | 0:84d5aa80fd77 | 23 | //ITG-3200 sensitivity is 14.375 LSB/(degrees/sec). |
Strikewolf | 0:84d5aa80fd77 | 24 | #define GYROSCOPE_GAIN (1 / 14.375) |
Strikewolf | 0:84d5aa80fd77 | 25 | //Full scale resolution on the ADXL345 is 4mg/LSB. |
Strikewolf | 0:84d5aa80fd77 | 26 | #define ACCELEROMETER_GAIN (0.004 * g0) |
Strikewolf | 0:84d5aa80fd77 | 27 | //Sampling gyroscope at 200Hz. |
Strikewolf | 0:84d5aa80fd77 | 28 | #define GYRO_RATE 0.005 |
Strikewolf | 0:84d5aa80fd77 | 29 | //Sampling accelerometer at 200Hz. |
Strikewolf | 0:84d5aa80fd77 | 30 | #define ACC_RATE 0.005 |
Strikewolf | 0:84d5aa80fd77 | 31 | //Updating filter at 40Hz. |
Strikewolf | 0:84d5aa80fd77 | 32 | #define FILTER_RATE 0.1 |
Strikewolf | 0:84d5aa80fd77 | 33 | |
Strikewolf | 0:84d5aa80fd77 | 34 | //At rest the gyroscope is centred around 0 and goes between about |
Strikewolf | 0:84d5aa80fd77 | 35 | //-5 and 5 counts. As 1 degrees/sec is ~15 LSB, error is roughly |
Strikewolf | 0:84d5aa80fd77 | 36 | //5/15 = 0.3 degrees/sec. |
Strikewolf | 0:84d5aa80fd77 | 37 | IMUfilter imuFilter(FILTER_RATE, 0.3); |
Strikewolf | 0:84d5aa80fd77 | 38 | ADXL345_I2C accelerometer(p28, p27); |
Strikewolf | 0:84d5aa80fd77 | 39 | ITG3200 gyroscope(p28, p27); |
Strikewolf | 0:84d5aa80fd77 | 40 | Ticker accelerometerTicker; |
Strikewolf | 0:84d5aa80fd77 | 41 | Ticker gyroscopeTicker; |
Strikewolf | 0:84d5aa80fd77 | 42 | Ticker filterTicker; |
Strikewolf | 0:84d5aa80fd77 | 43 | |
Strikewolf | 0:84d5aa80fd77 | 44 | //Offsets for the gyroscope. |
Strikewolf | 0:84d5aa80fd77 | 45 | //The readings we take when the gyroscope is stationary won't be 0, so we'll |
Strikewolf | 0:84d5aa80fd77 | 46 | //average a set of readings we do get when the gyroscope is stationary and |
Strikewolf | 0:84d5aa80fd77 | 47 | //take those away from subsequent readings to ensure the gyroscope is offset |
Strikewolf | 0:84d5aa80fd77 | 48 | //or "biased" to 0. |
Strikewolf | 0:84d5aa80fd77 | 49 | double w_xBias; |
Strikewolf | 0:84d5aa80fd77 | 50 | double w_yBias; |
Strikewolf | 0:84d5aa80fd77 | 51 | double w_zBias; |
Strikewolf | 0:84d5aa80fd77 | 52 | |
Strikewolf | 0:84d5aa80fd77 | 53 | //Offsets for the accelerometer. |
Strikewolf | 0:84d5aa80fd77 | 54 | //Same as with the gyroscope. |
Strikewolf | 0:84d5aa80fd77 | 55 | double a_xBias; |
Strikewolf | 0:84d5aa80fd77 | 56 | double a_yBias; |
Strikewolf | 0:84d5aa80fd77 | 57 | double a_zBias; |
Strikewolf | 0:84d5aa80fd77 | 58 | |
Strikewolf | 0:84d5aa80fd77 | 59 | //Accumulators used for oversampling and then averaging. |
Strikewolf | 0:84d5aa80fd77 | 60 | volatile double a_xAccumulator = 0; |
Strikewolf | 0:84d5aa80fd77 | 61 | volatile double a_yAccumulator = 0; |
Strikewolf | 0:84d5aa80fd77 | 62 | volatile double a_zAccumulator = 0; |
Strikewolf | 0:84d5aa80fd77 | 63 | volatile double w_xAccumulator = 0; |
Strikewolf | 0:84d5aa80fd77 | 64 | volatile double w_yAccumulator = 0; |
Strikewolf | 0:84d5aa80fd77 | 65 | volatile double w_zAccumulator = 0; |
Strikewolf | 0:84d5aa80fd77 | 66 | |
Strikewolf | 0:84d5aa80fd77 | 67 | //Accelerometer and gyroscope readings for x, y, z axes. |
Strikewolf | 0:84d5aa80fd77 | 68 | volatile double a_x; |
Strikewolf | 0:84d5aa80fd77 | 69 | volatile double a_y; |
Strikewolf | 0:84d5aa80fd77 | 70 | volatile double a_z; |
Strikewolf | 0:84d5aa80fd77 | 71 | volatile double w_x; |
Strikewolf | 0:84d5aa80fd77 | 72 | volatile double w_y; |
Strikewolf | 0:84d5aa80fd77 | 73 | volatile double w_z; |
Strikewolf | 0:84d5aa80fd77 | 74 | |
Strikewolf | 0:84d5aa80fd77 | 75 | //Buffer for accelerometer readings. |
Strikewolf | 0:84d5aa80fd77 | 76 | int readings[3]; |
Strikewolf | 0:84d5aa80fd77 | 77 | //Number of accelerometer samples we're on. |
Strikewolf | 0:84d5aa80fd77 | 78 | int accelerometerSamples = 0; |
Strikewolf | 0:84d5aa80fd77 | 79 | //Number of gyroscope samples we're on. |
Strikewolf | 0:84d5aa80fd77 | 80 | int gyroscopeSamples = 0; |
Strikewolf | 0:84d5aa80fd77 | 81 | |
Strikewolf | 0:84d5aa80fd77 | 82 | /** |
Strikewolf | 0:84d5aa80fd77 | 83 | * Prototypes |
Strikewolf | 0:84d5aa80fd77 | 84 | */ |
Strikewolf | 0:84d5aa80fd77 | 85 | //Set up the ADXL345 appropriately. |
Strikewolf | 0:84d5aa80fd77 | 86 | void initializeAcceleromter(void); |
Strikewolf | 0:84d5aa80fd77 | 87 | //Calculate the null bias. |
Strikewolf | 0:84d5aa80fd77 | 88 | void calibrateAccelerometer(void); |
Strikewolf | 0:84d5aa80fd77 | 89 | //Take a set of samples and average them. |
Strikewolf | 0:84d5aa80fd77 | 90 | void sampleAccelerometer(void); |
Strikewolf | 0:84d5aa80fd77 | 91 | //Set up the ITG3200 appropriately. |
Strikewolf | 0:84d5aa80fd77 | 92 | void initializeGyroscope(void); |
Strikewolf | 0:84d5aa80fd77 | 93 | //Calculate the null bias. |
Strikewolf | 0:84d5aa80fd77 | 94 | void calibrateGyroscope(void); |
Strikewolf | 0:84d5aa80fd77 | 95 | //Take a set of samples and average them. |
Strikewolf | 0:84d5aa80fd77 | 96 | void sampleGyroscope(void); |
Strikewolf | 0:84d5aa80fd77 | 97 | //Update the filter and calculate the Euler angles. |
Strikewolf | 0:84d5aa80fd77 | 98 | void filter(void); |
Strikewolf | 0:84d5aa80fd77 | 99 | |
Strikewolf | 0:84d5aa80fd77 | 100 | void initializeAccelerometer(void) |
Strikewolf | 0:84d5aa80fd77 | 101 | { |
Strikewolf | 0:84d5aa80fd77 | 102 | |
Strikewolf | 0:84d5aa80fd77 | 103 | //Go into standby mode to configure the device. |
Strikewolf | 0:84d5aa80fd77 | 104 | accelerometer.setPowerControl(0x00); |
Strikewolf | 0:84d5aa80fd77 | 105 | //Full resolution, +/-16g, 4mg/LSB. |
Strikewolf | 0:84d5aa80fd77 | 106 | accelerometer.setDataFormatControl(0x0B); |
Strikewolf | 0:84d5aa80fd77 | 107 | //200Hz data rate. |
Strikewolf | 0:84d5aa80fd77 | 108 | accelerometer.setDataRate(ADXL345_200HZ); |
Strikewolf | 0:84d5aa80fd77 | 109 | //Measurement mode. |
Strikewolf | 0:84d5aa80fd77 | 110 | accelerometer.setPowerControl(0x08); |
Strikewolf | 0:84d5aa80fd77 | 111 | //See http://www.analog.com/static/imported-files/application_notes/AN-1077.pdf |
Strikewolf | 0:84d5aa80fd77 | 112 | wait_ms(22); |
Strikewolf | 0:84d5aa80fd77 | 113 | |
Strikewolf | 0:84d5aa80fd77 | 114 | } |
Strikewolf | 0:84d5aa80fd77 | 115 | |
Strikewolf | 0:84d5aa80fd77 | 116 | void sampleAccelerometer(void) |
Strikewolf | 0:84d5aa80fd77 | 117 | { |
Strikewolf | 0:84d5aa80fd77 | 118 | |
Strikewolf | 0:84d5aa80fd77 | 119 | //Have we taken enough samples? |
Strikewolf | 0:84d5aa80fd77 | 120 | if (accelerometerSamples == SAMPLES) { |
Strikewolf | 0:84d5aa80fd77 | 121 | |
Strikewolf | 0:84d5aa80fd77 | 122 | //Average the samples, remove the bias, and calculate the acceleration |
Strikewolf | 0:84d5aa80fd77 | 123 | //in m/s/s. |
Strikewolf | 0:84d5aa80fd77 | 124 | a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN; |
Strikewolf | 0:84d5aa80fd77 | 125 | a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN; |
Strikewolf | 0:84d5aa80fd77 | 126 | a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN; |
Strikewolf | 0:84d5aa80fd77 | 127 | |
Strikewolf | 0:84d5aa80fd77 | 128 | a_xAccumulator = 0; |
Strikewolf | 0:84d5aa80fd77 | 129 | a_yAccumulator = 0; |
Strikewolf | 0:84d5aa80fd77 | 130 | a_zAccumulator = 0; |
Strikewolf | 0:84d5aa80fd77 | 131 | accelerometerSamples = 0; |
Strikewolf | 0:84d5aa80fd77 | 132 | |
Strikewolf | 0:84d5aa80fd77 | 133 | } else { |
Strikewolf | 0:84d5aa80fd77 | 134 | //Take another sample. |
Strikewolf | 0:84d5aa80fd77 | 135 | accelerometer.getOutput(readings); |
Strikewolf | 0:84d5aa80fd77 | 136 | |
Strikewolf | 0:84d5aa80fd77 | 137 | a_xAccumulator += (int16_t) readings[0]; |
Strikewolf | 0:84d5aa80fd77 | 138 | a_yAccumulator += (int16_t) readings[1]; |
Strikewolf | 0:84d5aa80fd77 | 139 | a_zAccumulator += (int16_t) readings[2]; |
Strikewolf | 0:84d5aa80fd77 | 140 | |
Strikewolf | 0:84d5aa80fd77 | 141 | accelerometerSamples++; |
Strikewolf | 0:84d5aa80fd77 | 142 | |
Strikewolf | 0:84d5aa80fd77 | 143 | } |
Strikewolf | 0:84d5aa80fd77 | 144 | |
Strikewolf | 0:84d5aa80fd77 | 145 | } |
Strikewolf | 0:84d5aa80fd77 | 146 | |
Strikewolf | 0:84d5aa80fd77 | 147 | void calibrateAccelerometer(void) |
Strikewolf | 0:84d5aa80fd77 | 148 | { |
Strikewolf | 0:84d5aa80fd77 | 149 | |
Strikewolf | 0:84d5aa80fd77 | 150 | a_xAccumulator = 0; |
Strikewolf | 0:84d5aa80fd77 | 151 | a_yAccumulator = 0; |
Strikewolf | 0:84d5aa80fd77 | 152 | a_zAccumulator = 0; |
Strikewolf | 0:84d5aa80fd77 | 153 | |
Strikewolf | 0:84d5aa80fd77 | 154 | //Take a number of readings and average them |
Strikewolf | 0:84d5aa80fd77 | 155 | //to calculate the zero g offset. |
Strikewolf | 0:84d5aa80fd77 | 156 | for (int i = 0; i < CALIBRATION_SAMPLES; i++) { |
Strikewolf | 0:84d5aa80fd77 | 157 | |
Strikewolf | 0:84d5aa80fd77 | 158 | accelerometer.getOutput(readings); |
Strikewolf | 0:84d5aa80fd77 | 159 | |
Strikewolf | 0:84d5aa80fd77 | 160 | a_xAccumulator += (int16_t) readings[0]; |
Strikewolf | 0:84d5aa80fd77 | 161 | a_yAccumulator += (int16_t) readings[1]; |
Strikewolf | 0:84d5aa80fd77 | 162 | a_zAccumulator += (int16_t) readings[2]; |
Strikewolf | 0:84d5aa80fd77 | 163 | |
Strikewolf | 0:84d5aa80fd77 | 164 | wait(ACC_RATE); |
Strikewolf | 0:84d5aa80fd77 | 165 | |
Strikewolf | 0:84d5aa80fd77 | 166 | } |
Strikewolf | 0:84d5aa80fd77 | 167 | |
Strikewolf | 0:84d5aa80fd77 | 168 | a_xAccumulator /= CALIBRATION_SAMPLES; |
Strikewolf | 0:84d5aa80fd77 | 169 | a_yAccumulator /= CALIBRATION_SAMPLES; |
Strikewolf | 0:84d5aa80fd77 | 170 | a_zAccumulator /= CALIBRATION_SAMPLES; |
Strikewolf | 0:84d5aa80fd77 | 171 | |
Strikewolf | 0:84d5aa80fd77 | 172 | //At 4mg/LSB, 250 LSBs is 1g. |
Strikewolf | 0:84d5aa80fd77 | 173 | a_xBias = a_xAccumulator; |
Strikewolf | 0:84d5aa80fd77 | 174 | a_yBias = a_yAccumulator; |
Strikewolf | 0:84d5aa80fd77 | 175 | a_zBias = (a_zAccumulator - 250); |
Strikewolf | 0:84d5aa80fd77 | 176 | |
Strikewolf | 0:84d5aa80fd77 | 177 | a_xAccumulator = 0; |
Strikewolf | 0:84d5aa80fd77 | 178 | a_yAccumulator = 0; |
Strikewolf | 0:84d5aa80fd77 | 179 | a_zAccumulator = 0; |
Strikewolf | 0:84d5aa80fd77 | 180 | |
Strikewolf | 0:84d5aa80fd77 | 181 | } |
Strikewolf | 0:84d5aa80fd77 | 182 | |
Strikewolf | 0:84d5aa80fd77 | 183 | void initializeGyroscope(void) |
Strikewolf | 0:84d5aa80fd77 | 184 | { |
Strikewolf | 0:84d5aa80fd77 | 185 | |
Strikewolf | 0:84d5aa80fd77 | 186 | //Low pass filter bandwidth of 42Hz. |
Strikewolf | 0:84d5aa80fd77 | 187 | gyroscope.setLpBandwidth(LPFBW_42HZ); |
Strikewolf | 0:84d5aa80fd77 | 188 | //Internal sample rate of 200Hz. (1kHz / 5). |
Strikewolf | 0:84d5aa80fd77 | 189 | gyroscope.setSampleRateDivider(4); |
Strikewolf | 0:84d5aa80fd77 | 190 | |
Strikewolf | 0:84d5aa80fd77 | 191 | } |
Strikewolf | 0:84d5aa80fd77 | 192 | |
Strikewolf | 0:84d5aa80fd77 | 193 | void calibrateGyroscope(void) |
Strikewolf | 0:84d5aa80fd77 | 194 | { |
Strikewolf | 0:84d5aa80fd77 | 195 | |
Strikewolf | 0:84d5aa80fd77 | 196 | w_xAccumulator = 0; |
Strikewolf | 0:84d5aa80fd77 | 197 | w_yAccumulator = 0; |
Strikewolf | 0:84d5aa80fd77 | 198 | w_zAccumulator = 0; |
Strikewolf | 0:84d5aa80fd77 | 199 | |
Strikewolf | 0:84d5aa80fd77 | 200 | //Take a number of readings and average them |
Strikewolf | 0:84d5aa80fd77 | 201 | //to calculate the gyroscope bias offset. |
Strikewolf | 0:84d5aa80fd77 | 202 | for (int i = 0; i < CALIBRATION_SAMPLES; i++) { |
Strikewolf | 0:84d5aa80fd77 | 203 | |
Strikewolf | 0:84d5aa80fd77 | 204 | w_xAccumulator += gyroscope.getGyroX(); |
Strikewolf | 0:84d5aa80fd77 | 205 | w_yAccumulator += gyroscope.getGyroY(); |
Strikewolf | 0:84d5aa80fd77 | 206 | w_zAccumulator += gyroscope.getGyroZ(); |
Strikewolf | 0:84d5aa80fd77 | 207 | wait(GYRO_RATE); |
Strikewolf | 0:84d5aa80fd77 | 208 | |
Strikewolf | 0:84d5aa80fd77 | 209 | } |
Strikewolf | 0:84d5aa80fd77 | 210 | |
Strikewolf | 0:84d5aa80fd77 | 211 | //Average the samples. |
Strikewolf | 0:84d5aa80fd77 | 212 | w_xAccumulator /= CALIBRATION_SAMPLES; |
Strikewolf | 0:84d5aa80fd77 | 213 | w_yAccumulator /= CALIBRATION_SAMPLES; |
Strikewolf | 0:84d5aa80fd77 | 214 | w_zAccumulator /= CALIBRATION_SAMPLES; |
Strikewolf | 0:84d5aa80fd77 | 215 | |
Strikewolf | 0:84d5aa80fd77 | 216 | w_xBias = w_xAccumulator; |
Strikewolf | 0:84d5aa80fd77 | 217 | w_yBias = w_yAccumulator; |
Strikewolf | 0:84d5aa80fd77 | 218 | w_zBias = w_zAccumulator; |
Strikewolf | 0:84d5aa80fd77 | 219 | |
Strikewolf | 0:84d5aa80fd77 | 220 | w_xAccumulator = 0; |
Strikewolf | 0:84d5aa80fd77 | 221 | w_yAccumulator = 0; |
Strikewolf | 0:84d5aa80fd77 | 222 | w_zAccumulator = 0; |
Strikewolf | 0:84d5aa80fd77 | 223 | |
Strikewolf | 0:84d5aa80fd77 | 224 | } |
Strikewolf | 0:84d5aa80fd77 | 225 | |
Strikewolf | 0:84d5aa80fd77 | 226 | void sampleGyroscope(void) |
Strikewolf | 0:84d5aa80fd77 | 227 | { |
Strikewolf | 0:84d5aa80fd77 | 228 | |
Strikewolf | 0:84d5aa80fd77 | 229 | //Have we taken enough samples? |
Strikewolf | 0:84d5aa80fd77 | 230 | if (gyroscopeSamples == SAMPLES) { |
Strikewolf | 0:84d5aa80fd77 | 231 | |
Strikewolf | 0:84d5aa80fd77 | 232 | //Average the samples, remove the bias, and calculate the angular |
Strikewolf | 0:84d5aa80fd77 | 233 | //velocity in rad/s. |
Strikewolf | 0:84d5aa80fd77 | 234 | w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN); |
Strikewolf | 0:84d5aa80fd77 | 235 | w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN); |
Strikewolf | 0:84d5aa80fd77 | 236 | w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN); |
Strikewolf | 0:84d5aa80fd77 | 237 | |
Strikewolf | 0:84d5aa80fd77 | 238 | w_xAccumulator = 0; |
Strikewolf | 0:84d5aa80fd77 | 239 | w_yAccumulator = 0; |
Strikewolf | 0:84d5aa80fd77 | 240 | w_zAccumulator = 0; |
Strikewolf | 0:84d5aa80fd77 | 241 | gyroscopeSamples = 0; |
Strikewolf | 0:84d5aa80fd77 | 242 | |
Strikewolf | 0:84d5aa80fd77 | 243 | } else { |
Strikewolf | 0:84d5aa80fd77 | 244 | //Take another sample. |
Strikewolf | 0:84d5aa80fd77 | 245 | w_xAccumulator += gyroscope.getGyroX(); |
Strikewolf | 0:84d5aa80fd77 | 246 | w_yAccumulator += gyroscope.getGyroY(); |
Strikewolf | 0:84d5aa80fd77 | 247 | w_zAccumulator += gyroscope.getGyroZ(); |
Strikewolf | 0:84d5aa80fd77 | 248 | |
Strikewolf | 0:84d5aa80fd77 | 249 | gyroscopeSamples++; |
Strikewolf | 0:84d5aa80fd77 | 250 | |
Strikewolf | 0:84d5aa80fd77 | 251 | } |
Strikewolf | 0:84d5aa80fd77 | 252 | |
Strikewolf | 0:84d5aa80fd77 | 253 | } |
Strikewolf | 0:84d5aa80fd77 | 254 | |
Strikewolf | 0:84d5aa80fd77 | 255 | void filter(void) |
Strikewolf | 0:84d5aa80fd77 | 256 | { |
Strikewolf | 0:84d5aa80fd77 | 257 | |
Strikewolf | 0:84d5aa80fd77 | 258 | //Update the filter variables. |
Strikewolf | 0:84d5aa80fd77 | 259 | imuFilter.updateFilter(w_y, w_x, w_z, a_y, a_x, a_z); |
Strikewolf | 0:84d5aa80fd77 | 260 | //Calculate the new Euler angles. |
Strikewolf | 0:84d5aa80fd77 | 261 | imuFilter.computeEuler(); |
Strikewolf | 0:84d5aa80fd77 | 262 | |
Strikewolf | 0:84d5aa80fd77 | 263 | } |
Strikewolf | 0:84d5aa80fd77 | 264 | |
Strikewolf | 0:84d5aa80fd77 | 265 | void rpy_init() |
Strikewolf | 0:84d5aa80fd77 | 266 | { |
Strikewolf | 0:84d5aa80fd77 | 267 | //Initialize inertial sensors. |
Strikewolf | 0:84d5aa80fd77 | 268 | initializeAccelerometer(); |
Strikewolf | 0:84d5aa80fd77 | 269 | calibrateAccelerometer(); |
Strikewolf | 0:84d5aa80fd77 | 270 | initializeGyroscope(); |
Strikewolf | 0:84d5aa80fd77 | 271 | //calibrateGyroscope(); |
Strikewolf | 0:84d5aa80fd77 | 272 | |
Strikewolf | 0:84d5aa80fd77 | 273 | |
Strikewolf | 0:84d5aa80fd77 | 274 | /* //Set up timers. |
Strikewolf | 0:84d5aa80fd77 | 275 | //Accelerometer data rate is 200Hz, so we'll sample at this speed. |
Strikewolf | 0:84d5aa80fd77 | 276 | accelerometerTicker.attach(&sampleAccelerometer, 0.005); |
Strikewolf | 0:84d5aa80fd77 | 277 | //Gyroscope data rate is 200Hz, so we'll sample at this speed. |
Strikewolf | 0:84d5aa80fd77 | 278 | gyroscopeTicker.attach(&sampleGyroscope, 0.005); |
Strikewolf | 0:84d5aa80fd77 | 279 | //Update the filter variables at the correct rate. |
Strikewolf | 0:84d5aa80fd77 | 280 | filterTicker.attach(&filter, FILTER_RATE);*/ |
Strikewolf | 0:84d5aa80fd77 | 281 | } |