Embed:
(wiki syntax)
Show/hide line numbers
IMUfilter.h
00001 /** 00002 * @author Aaron Berk 00003 * 00004 * @section LICENSE 00005 * 00006 * Copyright (c) 2010 ARM Limited 00007 * 00008 * Permission is hereby granted, free of charge, to any person obtaining a copy 00009 * of this software and associated documentation files (the "Software"), to deal 00010 * in the Software without restriction, including without limitation the rights 00011 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 00012 * copies of the Software, and to permit persons to whom the Software is 00013 * furnished to do so, subject to the following conditions: 00014 * 00015 * The above copyright notice and this permission notice shall be included in 00016 * all copies or substantial portions of the Software. 00017 * 00018 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 00019 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 00020 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 00021 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 00022 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 00023 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 00024 * THE SOFTWARE. 00025 * 00026 * @section DESCRIPTION 00027 * 00028 * IMU orientation filter developed by Sebastian Madgwick. 00029 * 00030 * Find more details about his paper here: 00031 * 00032 * http://code.google.com/p/imumargalgorithm30042010sohm/ 00033 */ 00034 00035 #ifndef IMU_FILTER_H 00036 #define IMU_FILTER_H 00037 00038 /** 00039 * Includes 00040 */ 00041 #include "mbed.h" 00042 00043 /** 00044 * Defines 00045 */ 00046 #define PI 3.1415926536 00047 00048 /** 00049 * IMU orientation filter. 00050 */ 00051 class IMUfilter { 00052 00053 public: 00054 00055 /** 00056 * Constructor. 00057 * 00058 * Initializes filter variables. 00059 * 00060 * @param rate The rate at which the filter should be updated. 00061 * @param gyroscopeMeasurementError The error of the gyroscope in degrees 00062 * per second. This used to calculate a tuning constant for the filter. 00063 * Try changing this value if there are jittery readings, or they change 00064 * too much or too fast when rotating the IMU. 00065 */ 00066 IMUfilter(double rate, double gyroscopeMeasurementError); 00067 00068 /** 00069 * Update the filter variables. 00070 * 00071 * @param w_x X-axis gyroscope reading in rad/s. 00072 * @param w_y Y-axis gyroscope reading in rad/s. 00073 * @param w_z Z-axis gyroscope reading in rad/s. 00074 * @param a_x X-axis accelerometer reading in m/s/s. 00075 * @param a_y Y-axis accelerometer reading in m/s/s. 00076 * @param a_z Z-axis accelerometer reading in m/s/s. 00077 */ 00078 void updateFilter(double w_x, double w_y, double w_z, 00079 double a_x, double a_y, double a_z); 00080 00081 /** 00082 * Compute the Euler angles based on the current filter data. 00083 */ 00084 void computeEuler(void); 00085 00086 /** 00087 * Get the current roll. 00088 * 00089 * @return The current roll angle in radians. 00090 */ 00091 double getRoll(void); 00092 00093 /** 00094 * Get the current pitch. 00095 * 00096 * @return The current pitch angle in radians. 00097 */ 00098 double getPitch(void); 00099 00100 /** 00101 * Get the current yaw. 00102 * 00103 * @return The current yaw angle in radians. 00104 */ 00105 double getYaw(void); 00106 00107 /** 00108 * Get the current Gyro error rate. 00109 * 00110 * @return The Gyro error rate setting. 00111 */ 00112 double getGyroError(void); 00113 00114 /** 00115 * Set the Gyro error rate. 00116 * The filter resets during calling this routine. 00117 */ 00118 void setGyroError(double gyroscopeMeasurementError); 00119 00120 /** 00121 * Reset the filter. 00122 */ 00123 void reset(void); 00124 00125 private: 00126 00127 int firstUpdate; 00128 00129 //Quaternion orientation of earth frame relative to auxiliary frame. 00130 double AEq_1; 00131 double AEq_2; 00132 double AEq_3; 00133 double AEq_4; 00134 00135 //Estimated orientation quaternion elements with initial conditions. 00136 double SEq_1; 00137 double SEq_2; 00138 double SEq_3; 00139 double SEq_4; 00140 00141 //Sampling period 00142 double deltat; 00143 00144 //Gyroscope measurement error (in degrees per second). 00145 double gyroMeasError; 00146 00147 //Compute beta (filter tuning constant.. 00148 double beta; 00149 00150 double phi; 00151 double theta; 00152 double psi; 00153 00154 }; 00155 00156 #endif /* IMU_FILTER_H */
Generated on Fri Jul 15 2022 09:19:09 by 1.7.2