imu01c

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers LSM303D.h Source File

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 };