LSM303D.cpp

00001 /** Tilt-compensated compass interface Library for the STMicro LSM303D 3-axis magnetometer, 3-axis acceleromter
00002  *
00003  * Based on
00004  * 
00005  * Michael Shimniok http://bot-thoughts.com
00006  *
00007  * test program by tosihisa and 
00008  *
00009  * Pololu sample library for LSM303DLH breakout by ryantm:
00010  *
00011  * Copyright (c) 2011 Pololu Corporation. For more information, see
00012  *
00013  * http://www.pololu.com/
00014  * http://forum.pololu.com/
00015  *
00016  * Permission is hereby granted, free of charge, to any person
00017  * obtaining a copy of this software and associated documentation
00018  * files (the "Software"), to deal in the Software without
00019  * restriction, including without limitation the rights to use,
00020  * copy, modify, merge, publish, distribute, sublicense, and/or sell
00021  * copies of the Software, and to permit persons to whom the
00022  * Software is furnished to do so, subject to the following
00023  * conditions:
00024  *
00025  * The above copyright notice and this permission notice shall be
00026  * included in all copies or substantial portions of the Software.
00027  * 
00028  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
00029  * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
00030  * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
00031  * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
00032  * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
00033  * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
00034  * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
00035  * OTHER DEALINGS IN THE SOFTWARE.
00036  */
00037  
00038 #include "mbed.h"
00039 #include "LSM303D.h"
00040 
00041 #ifndef M_PI
00042 #define M_PI 3.14159265358979323846
00043 #endif
00044 
00045 #define FILTER_SHIFT 1      // used in filtering acceleromter readings
00046 
00047 const int addr_ls303d = 0x3A;
00048 
00049 enum REG_ADDRS {
00050     /* --- Mag --- */
00051     OUT_TEMP    = 0x05,
00052     OUT_X_M     = 0x08,
00053     OUT_Y_M     = 0x0A,
00054     OUT_Z_M     = 0x0C,
00055     /* --- Acc --- */
00056     CTRL_REG1_A = 0x20,
00057     CTRL_REG2_A = 0x21,
00058     CTRL_REG5_A = 0x24,
00059     CTRL_REG6_A = 0x25,
00060     CTRL_REG7_A = 0x26,
00061     OUT_X_A     = 0x28,
00062     OUT_Y_A     = 0x2A,
00063     OUT_Z_A     = 0x2C,
00064 };
00065 
00066 bool LSM303D::write_reg(int addr_i2c,int addr_reg, char v)
00067 {
00068     char data[2] = {addr_reg, v}; 
00069     return LSM303D::_compass.write(addr_i2c, data, 2) == 0;
00070 }
00071 
00072 bool LSM303D::read_reg(int addr_i2c,int addr_reg, char *v)
00073 {
00074     char data = addr_reg; 
00075     bool result = false;
00076     
00077     __disable_irq();
00078     if ((_compass.write(addr_i2c, &data, 1) == 0) && (_compass.read(addr_i2c, &data, 1) == 0)){
00079         *v = data;
00080         result = true;
00081     }
00082     __enable_irq();
00083     return result;
00084 }
00085 
00086 
00087 
00088 LSM303D::LSM303D(PinName sda, PinName scl):
00089     _compass(sda, scl), _offset_x(0), _offset_y(0), _offset_z(0), _scale_x(1), _scale_y(1), _scale_z(1), _filt_ax(0), _filt_ay(0), _filt_az(9000)
00090 {
00091     char reg_v;
00092     //_compass.frequency(100000);
00093     
00094     //Chip Setup
00095     
00096     //CTRL 0 (1Fh)all Default
00097     
00098     //CTRL 1 (20h)   
00099     reg_v = 0;
00100     reg_v |= 0x04 << 4;     /* ACC 25Hz Autosample  */
00101     reg_v |= 0x07;          /* X/Y/Z axis enable. */
00102     write_reg(addr_ls303d,CTRL_REG1_A,reg_v); //
00103     reg_v = 0;
00104     read_reg(addr_ls303d,CTRL_REG1_A,&reg_v);
00105     
00106     //CTRL 2 (21h)
00107     reg_v = 0x00;          
00108     reg_v |= 0x03 << 6;     /* 50Hz Antialias Filter */
00109     reg_v |= 0x00 << 3;     /* +/- 2g */
00110     write_reg(addr_ls303d,CTRL_REG2_A,reg_v);
00111     
00112     //CTRL 3 (22h) all Default
00113     
00114     //CTRL 4 (23h) all Default
00115     
00116     //CTRL 5 (24h)
00117     reg_v = 0x00;          
00118     reg_v |= 0x01 << 7;     /* Temp Sensor Enable 1*/
00119     reg_v |= 0x03 << 5;     /* Mag high Res  3*/
00120     reg_v |= 0x03 << 2;     /* Mag 25Hz Autosample*/
00121     write_reg(addr_ls303d,CTRL_REG5_A,reg_v);
00122     
00123     //CTRL 6 (25h)
00124     reg_v = 0x00;          
00125     reg_v |= 0x00 << 5;     /* +-2 Gauss*/
00126     write_reg(addr_ls303d,CTRL_REG6_A,reg_v); //25h
00127 
00128     //CTRL 7 (26h)
00129     reg_v = 0x00;          
00130     reg_v |= 0x00 << 0;     /* Mag Continuous Conversation*/
00131     write_reg(addr_ls303d,CTRL_REG7_A,reg_v); //26h
00132 
00133     min.x =-2000;
00134     min.y =-2000;
00135     min.z = 1000;
00136         
00137     max.x = -1000;
00138     max.y = -1000;
00139     max.z =  4000;
00140     
00141     spreed.x = 6000;
00142     spreed.y = 6000;
00143     spreed.z = 6000;
00144 }
00145 
00146 
00147 void LSM303D::setOffset(float x, float y, float z)
00148 {
00149     _offset_x = x;
00150     _offset_y = y;
00151     _offset_z = z;   
00152 }
00153 
00154 void LSM303D::setScale(float x, float y, float z)
00155 {
00156     _scale_x = x;
00157     _scale_y = y;
00158     _scale_z = z;
00159 }
00160 
00161 void LSM303D::set_vectors()
00162 {
00163 
00164     // Perform simple lowpass filtering
00165     // Intended to stabilize heading despite
00166     // device vibration such as on a UGV
00167     _filt_ax += acc_raw.x - (_filt_ax >> FILTER_SHIFT);
00168     _filt_ay += acc_raw.y - (_filt_ay >> FILTER_SHIFT);
00169     _filt_az += acc_raw.z - (_filt_az >> FILTER_SHIFT);
00170 
00171     acc.x = (float) (_filt_ax >> FILTER_SHIFT);
00172     acc.y = (float) (_filt_ay >> FILTER_SHIFT);
00173     acc.z = (float) (_filt_az >> FILTER_SHIFT);
00174     
00175     // offset and scale
00176     //mag.x = (3 * mag.x + ((mag_raw.x + _offset_x) * _scale_x))/4;
00177     //mag.y = (3 * mag.y + ((mag_raw.y + _offset_y) * _scale_y))/4;
00178     //mag.z = (3 * mag.z + ((mag_raw.z + _offset_z) * _scale_z))/4; 
00179     mag.x = (mag.x + ((mag_raw.x + (((max.x - min.x)/2) - max.x)) * (10000/(max.x - min.x))) )/2;
00180     mag.y = (mag.y + ((mag_raw.y + (((max.y - min.y)/2) - max.y)) * (10000/(max.y - min.y))) )/2;
00181     mag.z = (mag.z + ((mag_raw.z + (((max.z - min.z)/2) - max.z)) * (10000/(max.z - min.z))) )/2;
00182    
00183     vector_normalize(&mag);  
00184 }
00185 
00186 void LSM303D::read()
00187 {
00188     char data[1] = { OUT_X_A | (1<<7)}; //Page 23 In order to read multiple bytes, it is necessary to assert the most significant bit of the subaddress field. In other words, SUB(7) must be equal to ‘1’ while SUB(6-0) represents the address of the first register to be read.
00189     char out[6] = {0,0,0,0,0,0};    
00190     
00191     _compass.write( addr_ls303d, data,1);
00192     _compass.read( addr_ls303d, out, 6);
00193     
00194     acc_raw.x= short( (((short)out[1]) << 8) | out[0] );
00195     acc_raw.y= short( (((short)out[3]) << 8) | out[2] );
00196     acc_raw.z= short( (((short)out[5]) << 8) | out[4] );   
00197 
00198     data[0] =  (OUT_X_M | (1<<7));
00199 
00200     _compass.write( addr_ls303d, data, 1 );
00201     _compass.read ( addr_ls303d, out, 6 );
00202     
00203     mag_raw.x= short( (((short)out[1]) << 8)| out[0]);
00204     mag_raw.y= short( (((short)out[3]) << 8)| out[2]);
00205     mag_raw.z= short( (((short)out[5]) << 8)| out[4]); 
00206   
00207     set_vectors();
00208     
00209     calc_pos();
00210 }
00211 
00212 
00213 
00214 void LSM303D::calc_pos(void)
00215 {   
00216   float x;
00217   float y;
00218   float c;
00219     ////////////////////////////////////////////////
00220     // compute heading       
00221     ////////////////////////////////////////////////
00222     vector from = {1,0,0};
00223     vector temp_a = acc;
00224     
00225     // normalize
00226     vector_normalize(&temp_a);
00227 
00228     // compute E and N
00229     vector E;                           //vector East
00230     vector N;                           //vector Nord
00231     vector_cross(&mag,&temp_a,&E);
00232     vector_normalize(&E);
00233     vector_cross(&temp_a,&E,&N);
00234     
00235     // compute heading
00236     x = vector_dot(&E,&from);
00237     y = vector_dot(&N,&from);
00238     c = sqrt(x*x + y*y);
00239     //hdg = atan2(x, y)) * 180/M_PI;
00240     hdg = atan2(x, y); // * 180/M_PI;
00241     hdg = ((asin(fabs(x)/c) * (1 - fabs(sin(hdg)))) + (acos(fabs(y)/c) * fabs(sin(hdg))))* 180 / M_PI;
00242     if ((x > 0) && (y < 0)) hdg = 180 - hdg;
00243     if ((x < 0) && (y < 0)) hdg = 180 + hdg;
00244     if ((x < 0) && (y > 0)) hdg = 360 - hdg;
00245     
00246     //if (hdg <   0) hdg += 360;
00247     //if (hdg > 360) hdg -= 360;
00248     
00249     vector_norm_xz(&temp_a);
00250     pitch = (asin(temp_a.x) * 180/M_PI) + 25;
00251     
00252 }
00253 
00254 void LSM303D::set_limits(int mode)
00255 {
00256    if(mode == 1)                            //first cal.
00257     {
00258      if (max.x < mag_raw.x) max.x = mag_raw.x;
00259      if (min.x > mag_raw.x) min.x = mag_raw.x;
00260      if (max.y < mag_raw.y) max.y = mag_raw.y;
00261      if (min.y > mag_raw.y) min.y = mag_raw.y; 
00262      if (max.z < mag_raw.z) max.z = mag_raw.z;
00263      if (min.z > mag_raw.z) min.z = mag_raw.z;
00264    }
00265    if(mode == 2)                            //autorecal.
00266     {   
00267      if (max.x < mag_raw.x) max.x += 5;
00268      if (min.x > mag_raw.x) min.x -= 5;
00269      if (max.y < mag_raw.y) max.y += 5;
00270      if (min.y > mag_raw.y) min.y -= 5; 
00271      if (max.z < mag_raw.z) max.z += 5;
00272      if (min.z > mag_raw.z) min.z -= 5;  
00273     }
00274        
00275    if ((max.x - min.x) > spreed.x) {max.x--; min.x++;};
00276    if ((max.y - min.y) > spreed.y) {max.y--; min.y++;};
00277    if ((max.z - min.z) > spreed.z) {max.z--; min.z++;}; 
00278 }
00279 
00280 void LSM303D::frequency(int hz)
00281 {
00282     _compass.frequency(hz);
00283 }