Aaron Berk / Mbed 2 deprecated RS-EDP-RDS-Rover

Roving robot using the RS-EDP.

Dependencies:   mbed RSEDP_AM_MC1_lib SDFileSystem

IMU.h@1:ffef6386027b, 2010-08-26 (annotated)

Committer:
aberk
Date:
Thu Aug 26 14:41:08 2010 +0000
Revision:
1:ffef6386027b
Added additional comments and documentation.

Who changed what in which revision?

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