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 * Reset the filter. 00109 */ 00110 void reset(void); 00111 00112 private: 00113 00114 int firstUpdate; 00115 00116 //Quaternion orientation of earth frame relative to auxiliary frame. 00117 double AEq_1; 00118 double AEq_2; 00119 double AEq_3; 00120 double AEq_4; 00121 00122 //Estimated orientation quaternion elements with initial conditions. 00123 double SEq_1; 00124 double SEq_2; 00125 double SEq_3; 00126 double SEq_4; 00127 00128 //Sampling period 00129 double deltat; 00130 00131 //Gyroscope measurement error (in degrees per second). 00132 double gyroMeasError; 00133 00134 //Compute beta (filter tuning constant.. 00135 double beta; 00136 00137 double phi; 00138 double theta; 00139 double psi; 00140 00141 }; 00142 00143 #endif /* IMU_FILTER_H */
Generated on Sat Jul 16 2022 20:51:42 by 1.7.2