Gal Yaroslavsky / MicroMouse-v1

asdf

Dependencies:   L3GD20 LSM303DLHC mbed

IMU_Filter/IMUFilter.h@8:ce5b1bf38077, 2014-04-03 (annotated)

Committer:
goy5022
Date:
Thu Apr 03 23:58:04 2014 +0000
Revision:
8:ce5b1bf38077
Parent:
1:cfe6a6ad8dca 
asdf

Who changed what in which revision?

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