Aaron Berk / Mbed 2 deprecated IMU

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

Dependencies:   mbed

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?

UserRevisionLine numberNew 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 */