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

Roving robot using the RS-EDP.

Dependencies:   mbed RSEDP_AM_MC1_lib SDFileSystem

IMUfilter.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 orientation filter developed by Sebastian Madgwick.
aberk 1:ffef6386027b 27 *
aberk 1:ffef6386027b 28 * Find more details about his paper here:
aberk 1:ffef6386027b 29 *
aberk 1:ffef6386027b 30 * http://code.google.com/p/imumargalgorithm30042010sohm/
aberk 1:ffef6386027b 31 */
aberk 1:ffef6386027b 32
aberk 1:ffef6386027b 33 #ifndef IMU_FILTER_H
aberk 1:ffef6386027b 34 #define IMU_FILTER_H
aberk 1:ffef6386027b 35
aberk 1:ffef6386027b 36 /**
aberk 1:ffef6386027b 37 * Includes
aberk 1:ffef6386027b 38 */
aberk 1:ffef6386027b 39 #include "mbed.h"
aberk 1:ffef6386027b 40
aberk 1:ffef6386027b 41 /**
aberk 1:ffef6386027b 42 * Defines
aberk 1:ffef6386027b 43 */
aberk 1:ffef6386027b 44 #define PI 3.1415926536
aberk 1:ffef6386027b 45
aberk 1:ffef6386027b 46 /**
aberk 1:ffef6386027b 47 * IMU orientation filter.
aberk 1:ffef6386027b 48 */
aberk 1:ffef6386027b 49 class IMUfilter {
aberk 1:ffef6386027b 50
aberk 1:ffef6386027b 51 public:
aberk 1:ffef6386027b 52
aberk 1:ffef6386027b 53 /**
aberk 1:ffef6386027b 54 * Constructor.
aberk 1:ffef6386027b 55 *
aberk 1:ffef6386027b 56 * Initializes filter variables.
aberk 1:ffef6386027b 57 *
aberk 1:ffef6386027b 58 * @param rate The rate at which the filter should be updated.
aberk 1:ffef6386027b 59 * @param gyroscopeMeasurementError The error of the gyroscope in degrees
aberk 1:ffef6386027b 60 * per second. This used to calculate a tuning constant for the filter.
aberk 1:ffef6386027b 61 * Try changing this value if there are jittery readings, or they change
aberk 1:ffef6386027b 62 * too much or too fast when rotating the IMU.
aberk 1:ffef6386027b 63 */
aberk 1:ffef6386027b 64 IMUfilter(double rate, double gyroscopeMeasurementError);
aberk 1:ffef6386027b 65
aberk 1:ffef6386027b 66 /**
aberk 1:ffef6386027b 67 * Update the filter variables.
aberk 1:ffef6386027b 68 *
aberk 1:ffef6386027b 69 * @param w_x X-axis gyroscope reading in rad/s.
aberk 1:ffef6386027b 70 * @param w_y Y-axis gyroscope reading in rad/s.
aberk 1:ffef6386027b 71 * @param w_z Z-axis gyroscope reading in rad/s.
aberk 1:ffef6386027b 72 * @param a_x X-axis accelerometer reading in m/s/s.
aberk 1:ffef6386027b 73 * @param a_y Y-axis accelerometer reading in m/s/s.
aberk 1:ffef6386027b 74 * @param a_z Z-axis accelerometer reading in m/s/s.
aberk 1:ffef6386027b 75 */
aberk 1:ffef6386027b 76 void updateFilter(double w_x, double w_y, double w_z,
aberk 1:ffef6386027b 77 double a_x, double a_y, double a_z);
aberk 1:ffef6386027b 78
aberk 1:ffef6386027b 79 /**
aberk 1:ffef6386027b 80 * Compute the Euler angles based on the current filter data.
aberk 1:ffef6386027b 81 */
aberk 1:ffef6386027b 82 void computeEuler(void);
aberk 1:ffef6386027b 83
aberk 1:ffef6386027b 84 /**
aberk 1:ffef6386027b 85 * Get the current roll.
aberk 1:ffef6386027b 86 *
aberk 1:ffef6386027b 87 * @return The current roll angle in radians.
aberk 1:ffef6386027b 88 */
aberk 1:ffef6386027b 89 double getRoll(void);
aberk 1:ffef6386027b 90
aberk 1:ffef6386027b 91 /**
aberk 1:ffef6386027b 92 * Get the current pitch.
aberk 1:ffef6386027b 93 *
aberk 1:ffef6386027b 94 * @return The current pitch angle in radians.
aberk 1:ffef6386027b 95 */
aberk 1:ffef6386027b 96 double getPitch(void);
aberk 1:ffef6386027b 97
aberk 1:ffef6386027b 98 /**
aberk 1:ffef6386027b 99 * Get the current yaw.
aberk 1:ffef6386027b 100 *
aberk 1:ffef6386027b 101 * @return The current yaw angle in radians.
aberk 1:ffef6386027b 102 */
aberk 1:ffef6386027b 103 double getYaw(void);
aberk 1:ffef6386027b 104
aberk 1:ffef6386027b 105 /**
aberk 1:ffef6386027b 106 * Reset the filter.
aberk 1:ffef6386027b 107 */
aberk 1:ffef6386027b 108 void reset(void);
aberk 1:ffef6386027b 109
aberk 1:ffef6386027b 110 private:
aberk 1:ffef6386027b 111
aberk 1:ffef6386027b 112 int firstUpdate;
aberk 1:ffef6386027b 113
aberk 1:ffef6386027b 114 //Quaternion orientation of earth frame relative to auxiliary frame.
aberk 1:ffef6386027b 115 double AEq_1;
aberk 1:ffef6386027b 116 double AEq_2;
aberk 1:ffef6386027b 117 double AEq_3;
aberk 1:ffef6386027b 118 double AEq_4;
aberk 1:ffef6386027b 119
aberk 1:ffef6386027b 120 //Estimated orientation quaternion elements with initial conditions.
aberk 1:ffef6386027b 121 double SEq_1;
aberk 1:ffef6386027b 122 double SEq_2;
aberk 1:ffef6386027b 123 double SEq_3;
aberk 1:ffef6386027b 124 double SEq_4;
aberk 1:ffef6386027b 125
aberk 1:ffef6386027b 126 //Sampling period
aberk 1:ffef6386027b 127 double deltat;
aberk 1:ffef6386027b 128
aberk 1:ffef6386027b 129 //Gyroscope measurement error (in degrees per second).
aberk 1:ffef6386027b 130 double gyroMeasError;
aberk 1:ffef6386027b 131
aberk 1:ffef6386027b 132 //Compute beta (filter tuning constant..
aberk 1:ffef6386027b 133 double beta;
aberk 1:ffef6386027b 134
aberk 1:ffef6386027b 135 double phi;
aberk 1:ffef6386027b 136 double theta;
aberk 1:ffef6386027b 137 double psi;
aberk 1:ffef6386027b 138
aberk 1:ffef6386027b 139 };
aberk 1:ffef6386027b 140
aberk 1:ffef6386027b 141 #endif /* IMU_FILTER_H */