Committer:
nimbusgb
Date:
Sat Feb 05 18:57:55 2011 +0000
Revision:
0:6a5296aa7dbf
Child:
1:4b0e8441f099

        

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nimbusgb 0:6a5296aa7dbf 1 /**
nimbusgb 0:6a5296aa7dbf 2 * @author Aaron Berk
nimbusgb 0:6a5296aa7dbf 3 *
nimbusgb 0:6a5296aa7dbf 4 * @section LICENSE
nimbusgb 0:6a5296aa7dbf 5 *
nimbusgb 0:6a5296aa7dbf 6 * Copyright (c) 2010 ARM Limited
nimbusgb 0:6a5296aa7dbf 7 *
nimbusgb 0:6a5296aa7dbf 8 * Permission is hereby granted, free of charge, to any person obtaining a copy
nimbusgb 0:6a5296aa7dbf 9 * of this software and associated documentation files (the "Software"), to deal
nimbusgb 0:6a5296aa7dbf 10 * in the Software without restriction, including without limitation the rights
nimbusgb 0:6a5296aa7dbf 11 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
nimbusgb 0:6a5296aa7dbf 12 * copies of the Software, and to permit persons to whom the Software is
nimbusgb 0:6a5296aa7dbf 13 * furnished to do so, subject to the following conditions:
nimbusgb 0:6a5296aa7dbf 14 *
nimbusgb 0:6a5296aa7dbf 15 * The above copyright notice and this permission notice shall be included in
nimbusgb 0:6a5296aa7dbf 16 * all copies or substantial portions of the Software.
nimbusgb 0:6a5296aa7dbf 17 *
nimbusgb 0:6a5296aa7dbf 18 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
nimbusgb 0:6a5296aa7dbf 19 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
nimbusgb 0:6a5296aa7dbf 20 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
nimbusgb 0:6a5296aa7dbf 21 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
nimbusgb 0:6a5296aa7dbf 22 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
nimbusgb 0:6a5296aa7dbf 23 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
nimbusgb 0:6a5296aa7dbf 24 * THE SOFTWARE.
nimbusgb 0:6a5296aa7dbf 25 *
nimbusgb 0:6a5296aa7dbf 26 * @section DESCRIPTION
nimbusgb 0:6a5296aa7dbf 27 *
nimbusgb 0:6a5296aa7dbf 28 * IMU orientation filter developed by Sebastian Madgwick.
nimbusgb 0:6a5296aa7dbf 29 *
nimbusgb 0:6a5296aa7dbf 30 * Find more details about his paper here:
nimbusgb 0:6a5296aa7dbf 31 *
nimbusgb 0:6a5296aa7dbf 32 * http://code.google.com/p/imumargalgorithm30042010sohm/
nimbusgb 0:6a5296aa7dbf 33 */
nimbusgb 0:6a5296aa7dbf 34
nimbusgb 0:6a5296aa7dbf 35 #ifndef IMU_FILTER_H
nimbusgb 0:6a5296aa7dbf 36 #define IMU_FILTER_H
nimbusgb 0:6a5296aa7dbf 37
nimbusgb 0:6a5296aa7dbf 38 /**
nimbusgb 0:6a5296aa7dbf 39 * Includes
nimbusgb 0:6a5296aa7dbf 40 */
nimbusgb 0:6a5296aa7dbf 41 #include "mbed.h"
nimbusgb 0:6a5296aa7dbf 42
nimbusgb 0:6a5296aa7dbf 43 /**
nimbusgb 0:6a5296aa7dbf 44 * Defines
nimbusgb 0:6a5296aa7dbf 45 */
nimbusgb 0:6a5296aa7dbf 46 #define PI 3.1415926536
nimbusgb 0:6a5296aa7dbf 47
nimbusgb 0:6a5296aa7dbf 48 /**
nimbusgb 0:6a5296aa7dbf 49 * IMU orientation filter.
nimbusgb 0:6a5296aa7dbf 50 */
nimbusgb 0:6a5296aa7dbf 51 class IMUfilter {
nimbusgb 0:6a5296aa7dbf 52
nimbusgb 0:6a5296aa7dbf 53 public:
nimbusgb 0:6a5296aa7dbf 54
nimbusgb 0:6a5296aa7dbf 55 /**
nimbusgb 0:6a5296aa7dbf 56 * Constructor.
nimbusgb 0:6a5296aa7dbf 57 *
nimbusgb 0:6a5296aa7dbf 58 * Initializes filter variables.
nimbusgb 0:6a5296aa7dbf 59 *
nimbusgb 0:6a5296aa7dbf 60 * @param rate The rate at which the filter should be updated.
nimbusgb 0:6a5296aa7dbf 61 * @param gyroscopeMeasurementError The error of the gyroscope in degrees
nimbusgb 0:6a5296aa7dbf 62 * per second. This used to calculate a tuning constant for the filter.
nimbusgb 0:6a5296aa7dbf 63 * Try changing this value if there are jittery readings, or they change
nimbusgb 0:6a5296aa7dbf 64 * too much or too fast when rotating the IMU.
nimbusgb 0:6a5296aa7dbf 65 */
nimbusgb 0:6a5296aa7dbf 66 IMUfilter(double rate, double gyroscopeMeasurementError);
nimbusgb 0:6a5296aa7dbf 67
nimbusgb 0:6a5296aa7dbf 68 /**
nimbusgb 0:6a5296aa7dbf 69 * Update the filter variables.
nimbusgb 0:6a5296aa7dbf 70 *
nimbusgb 0:6a5296aa7dbf 71 * @param w_x X-axis gyroscope reading in rad/s.
nimbusgb 0:6a5296aa7dbf 72 * @param w_y Y-axis gyroscope reading in rad/s.
nimbusgb 0:6a5296aa7dbf 73 * @param w_z Z-axis gyroscope reading in rad/s.
nimbusgb 0:6a5296aa7dbf 74 * @param a_x X-axis accelerometer reading in m/s/s.
nimbusgb 0:6a5296aa7dbf 75 * @param a_y Y-axis accelerometer reading in m/s/s.
nimbusgb 0:6a5296aa7dbf 76 * @param a_z Z-axis accelerometer reading in m/s/s.
nimbusgb 0:6a5296aa7dbf 77 */
nimbusgb 0:6a5296aa7dbf 78 void updateFilter(double w_x, double w_y, double w_z,
nimbusgb 0:6a5296aa7dbf 79 double a_x, double a_y, double a_z);
nimbusgb 0:6a5296aa7dbf 80
nimbusgb 0:6a5296aa7dbf 81 /**
nimbusgb 0:6a5296aa7dbf 82 * Compute the Euler angles based on the current filter data.
nimbusgb 0:6a5296aa7dbf 83 */
nimbusgb 0:6a5296aa7dbf 84 void computeEuler(void);
nimbusgb 0:6a5296aa7dbf 85
nimbusgb 0:6a5296aa7dbf 86 /**
nimbusgb 0:6a5296aa7dbf 87 * Get the current roll.
nimbusgb 0:6a5296aa7dbf 88 *
nimbusgb 0:6a5296aa7dbf 89 * @return The current roll angle in radians.
nimbusgb 0:6a5296aa7dbf 90 */
nimbusgb 0:6a5296aa7dbf 91 double getRoll(void);
nimbusgb 0:6a5296aa7dbf 92
nimbusgb 0:6a5296aa7dbf 93 /**
nimbusgb 0:6a5296aa7dbf 94 * Get the current pitch.
nimbusgb 0:6a5296aa7dbf 95 *
nimbusgb 0:6a5296aa7dbf 96 * @return The current pitch angle in radians.
nimbusgb 0:6a5296aa7dbf 97 */
nimbusgb 0:6a5296aa7dbf 98 double getPitch(void);
nimbusgb 0:6a5296aa7dbf 99
nimbusgb 0:6a5296aa7dbf 100 /**
nimbusgb 0:6a5296aa7dbf 101 * Get the current yaw.
nimbusgb 0:6a5296aa7dbf 102 *
nimbusgb 0:6a5296aa7dbf 103 * @return The current yaw angle in radians.
nimbusgb 0:6a5296aa7dbf 104 */
nimbusgb 0:6a5296aa7dbf 105 double getYaw(void);
nimbusgb 0:6a5296aa7dbf 106
nimbusgb 0:6a5296aa7dbf 107 /**
nimbusgb 0:6a5296aa7dbf 108 * Reset the filter.
nimbusgb 0:6a5296aa7dbf 109 */
nimbusgb 0:6a5296aa7dbf 110 void reset(void);
nimbusgb 0:6a5296aa7dbf 111
nimbusgb 0:6a5296aa7dbf 112 private:
nimbusgb 0:6a5296aa7dbf 113
nimbusgb 0:6a5296aa7dbf 114 int firstUpdate;
nimbusgb 0:6a5296aa7dbf 115
nimbusgb 0:6a5296aa7dbf 116 //Quaternion orientation of earth frame relative to auxiliary frame.
nimbusgb 0:6a5296aa7dbf 117 double AEq_1;
nimbusgb 0:6a5296aa7dbf 118 double AEq_2;
nimbusgb 0:6a5296aa7dbf 119 double AEq_3;
nimbusgb 0:6a5296aa7dbf 120 double AEq_4;
nimbusgb 0:6a5296aa7dbf 121
nimbusgb 0:6a5296aa7dbf 122 //Estimated orientation quaternion elements with initial conditions.
nimbusgb 0:6a5296aa7dbf 123 double SEq_1;
nimbusgb 0:6a5296aa7dbf 124 double SEq_2;
nimbusgb 0:6a5296aa7dbf 125 double SEq_3;
nimbusgb 0:6a5296aa7dbf 126 double SEq_4;
nimbusgb 0:6a5296aa7dbf 127
nimbusgb 0:6a5296aa7dbf 128 //Sampling period
nimbusgb 0:6a5296aa7dbf 129 double deltat;
nimbusgb 0:6a5296aa7dbf 130
nimbusgb 0:6a5296aa7dbf 131 //Gyroscope measurement error (in degrees per second).
nimbusgb 0:6a5296aa7dbf 132 double gyroMeasError;
nimbusgb 0:6a5296aa7dbf 133
nimbusgb 0:6a5296aa7dbf 134 //Compute beta (filter tuning constant..
nimbusgb 0:6a5296aa7dbf 135 double beta;
nimbusgb 0:6a5296aa7dbf 136
nimbusgb 0:6a5296aa7dbf 137 double phi;
nimbusgb 0:6a5296aa7dbf 138 double theta;
nimbusgb 0:6a5296aa7dbf 139 double psi;
nimbusgb 0:6a5296aa7dbf 140
nimbusgb 0:6a5296aa7dbf 141 };
nimbusgb 0:6a5296aa7dbf 142
nimbusgb 0:6a5296aa7dbf 143 #endif /* IMU_FILTER_H */