jetfishteam / Mbed 2 deprecated robotic_fish_ver_4_9_pixy

My modifications/additions to the code

Dependencies:   ADXL345 ADXL345_I2C IMUfilter ITG3200 Servo fishgait mbed-rtos mbed pixy_cam

Fork of robotic_fish_ver_4_8 by jetfishteam

IMU.h@25:4f2f441eceec, 2014-07-11 (annotated)

Committer:
sandwich
Date:
Fri Jul 11 14:30:36 2014 +0000
Revision:
25:4f2f441eceec
Parent:
0:ff9bc5f69c57 
latest revision

Who changed what in which revision?

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