imu01c
Embed:
(wiki syntax)
Show/hide line numbers
LSM303D.h
00001 #include "mbed.h" 00002 #include "vector.h" 00003 00004 #ifndef M_PI 00005 #define M_PI 3.14159265358979323846 00006 #endif 00007 00008 /** Tilt-compensated compass interface Library for the STMicro LSM303D 3-axis magnetometer, 3-axis acceleromter 00009 * 00010 * Based on 00011 * 00012 * Michael Shimniok http://bot-thoughts.com 00013 * 00014 * test program by tosihisa and 00015 * 00016 * Pololu sample library for LSM303DLH breakout by ryantm: 00017 * 00018 * Copyright (c) 2011 Pololu Corporation. For more information, see 00019 * 00020 * http://www.pololu.com/ 00021 * http://forum.pololu.com/ 00022 * 00023 * Permission is hereby granted, free of charge, to any person 00024 * obtaining a copy of this software and associated documentation 00025 * files (the "Software"), to deal in the Software without 00026 * restriction, including without limitation the rights to use, 00027 * copy, modify, merge, publish, distribute, sublicense, and/or sell 00028 * copies of the Software, and to permit persons to whom the 00029 * Software is furnished to do so, subject to the following 00030 * conditions: 00031 * 00032 * The above copyright notice and this permission notice shall be 00033 * included in all copies or substantial portions of the Software. 00034 * 00035 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 00036 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES 00037 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 00038 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 00039 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 00040 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 00041 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR 00042 * OTHER DEALINGS IN THE SOFTWARE. 00043 * 00044 * @code 00045 * #include "mbed.h" 00046 * #include "LSM303D.h" 00047 * 00048 * Serial debug(USBTX,USBRX); 00049 * LSM303D compass(p28, p27); 00050 * 00051 * int main() 00052 * { 00053 * 00054 * int count; 00055 * 00056 * debug.baud(115200); 00057 * debug.printf("LSM303D Test\x0d\x0a"); 00058 * 00059 * compass.setOffset(460, 610, -290); 00060 * compass.setScale(0.93, 0.92, 1.00); 00061 * 00062 * while(1) 00063 * { 00064 * compass.read(); 00065 * count++; 00066 * if (count%10==0) debug.printf("Heading: %5.1f\n\r Pitch: %4.1f\n\r",compass.hdg, compass.pitch ); 00067 * wait(0.050); 00068 * } 00069 * } 00070 * @endcode 00071 */ 00072 class LSM303D { 00073 public: 00074 //! ACC Raw readings with x,y and z axis 00075 vector acc_raw; 00076 00077 //! MAG Raw readings with x,y and z axis 00078 vector mag_raw; 00079 00080 //! ACC Filtert readings with x,y and z axis 00081 vector acc; 00082 00083 //! MAG Normal readings with x,y and z axis 00084 vector mag; 00085 00086 //! Heading 00087 float hdg; 00088 00089 //! Pitch 00090 float pitch; 00091 00092 //! MAG Minimal readings 00093 vector min; 00094 00095 //! MAG Maximal readings 00096 vector max; 00097 00098 //! MAG Min-Max estimated range 00099 vector spreed; 00100 00101 /** Create a new interface for an LSM303D 00102 * 00103 * @param sda is the pin for the I2C SDA line 00104 * @param scl is the pin for the I2C SCL line 00105 */ 00106 LSM303D(PinName sda, PinName scl); 00107 00108 /** sets the x, y, and z offset corrections for hard iron calibration 00109 * 00110 * Calibration details here: 00111 * http://mbed.org/users/shimniok/notebook/quick-and-dirty-3d-compass-calibration/ 00112 * 00113 * If you gather raw magnetometer data and find, for example, x is offset 00114 * by hard iron by -20 then pass +20 to this member function to correct 00115 * for hard iron. 00116 * 00117 * @param x is the offset correction for the x axis 00118 * @param y is the offset correction for the y axis 00119 * @param z is the offset correction for the z axis 00120 */ 00121 void setOffset(float x, float y, float z); 00122 00123 /** sets the scale factor for the x, y, and z axes 00124 * 00125 * Calibratio details here: 00126 * http://mbed.org/users/shimniok/notebook/quick-and-dirty-3d-compass-calibration/ 00127 * 00128 * Sensitivity of the three axes is never perfectly identical and this 00129 * function can help to correct differences in sensitivity. You're 00130 * supplying a multipler such that x, y and z will be normalized to the 00131 * same max/min values 00132 */ 00133 void setScale(float x, float y, float z); 00134 00135 /** read the raw accelerometer and compass values 00136 * calc Heading and Pitch 00137 */ 00138 void read(); 00139 00140 00141 /** Set Mag Limits 00142 */ 00143 void set_limits(int mode); 00144 00145 00146 /** sets the I2C bus frequency 00147 * 00148 * @param frequency is the I2C bus/clock frequency, either standard (100000) or fast (400000) 00149 */ 00150 void frequency(int hz); 00151 00152 private: 00153 I2C _compass; 00154 float _offset_x; 00155 float _offset_y; 00156 float _offset_z; 00157 float _scale_x; 00158 float _scale_y; 00159 float _scale_z; 00160 long _filt_ax; 00161 long _filt_ay; 00162 long _filt_az; 00163 00164 00165 bool write_reg(int addr_i2c,int addr_reg, char v); 00166 bool read_reg(int addr_i2c,int addr_reg, char *v); 00167 void calc_pos(void); 00168 void set_vectors(void); 00169 00170 };
Generated on Tue Jul 12 2022 17:15:09 by 1.7.2