LSM303DLH.cpp

00001 /** LSM303DLH Interface Library
00002  *
00003  * Michael Shimniok http://bot-thoughts.com
00004  *
00005  * Based on test program by @tosihisa and 
00006  *
00007  * Pololu sample library for LSM303DLH breakout by ryantm:
00008  *
00009  * Copyright (c) 2011 Pololu Corporation. For more information, see
00010  *
00011  * http://www.pololu.com/
00012  * http://forum.pololu.com/
00013  *
00014  * Permission is hereby granted, free of charge, to any person
00015  * obtaining a copy of this software and associated documentation
00016  * files (the "Software"), to deal in the Software without
00017  * restriction, including without limitation the rights to use,
00018  * copy, modify, merge, publish, distribute, sublicense, and/or sell
00019  * copies of the Software, and to permit persons to whom the
00020  * Software is furnished to do so, subject to the following
00021  * conditions:
00022  *
00023  * The above copyright notice and this permission notice shall be
00024  * included in all copies or substantial portions of the Software.
00025  * 
00026  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
00027  * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
00028  * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
00029  * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
00030  * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
00031  * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
00032  * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
00033  * OTHER DEALINGS IN THE SOFTWARE.
00034  */
00035 #include "mbed.h"
00036 #include "LSM303DLH.h"
00037 
00038 #ifndef M_PI
00039 #define M_PI 3.14159265358979323846
00040 #endif
00041 
00042 #define FILTER_SHIFT 6      // used in filtering acceleromter readings
00043 
00044 const int addr_acc = 0x30;
00045 const int addr_mag = 0x3c;
00046 
00047 enum REG_ADDRS {
00048     /* --- Mag --- */
00049     CRA_REG_M   = 0x00,
00050     CRB_REG_M   = 0x01,
00051     MR_REG_M    = 0x02,
00052     OUT_X_M     = 0x03,
00053     OUT_Y_M     = 0x05,
00054     OUT_Z_M     = 0x07,
00055     /* --- Acc --- */
00056     CTRL_REG1_A = 0x20,
00057     CTRL_REG4_A = 0x23,
00058     OUT_X_A     = 0x28,
00059     OUT_Y_A     = 0x2A,
00060     OUT_Z_A     = 0x2C,
00061 };
00062 
00063 bool LSM303DLH::write_reg(int addr_i2c,int addr_reg, char v)
00064 {
00065     char data[2] = {addr_reg, v}; 
00066     return LSM303DLH::_compass.write(addr_i2c, data, 2) == 0;
00067 }
00068 
00069 bool LSM303DLH::read_reg(int addr_i2c,int addr_reg, char *v)
00070 {
00071     char data = addr_reg; 
00072     bool result = false;
00073     
00074     __disable_irq();
00075     if ((_compass.write(addr_i2c, &data, 1) == 0) && (_compass.read(addr_i2c, &data, 1) == 0)){
00076         *v = data;
00077         result = true;
00078     }
00079     __enable_irq();
00080     return result;
00081 }
00082 
00083 bool LSM303DLH::read_reg_short(int addr_i2c,int addr_reg, short *v)
00084 {
00085     char *pv = (char *)v;
00086     bool result;
00087     
00088     result =  read_reg(addr_i2c,addr_reg+0,pv+1);
00089     result &= read_reg(addr_i2c,addr_reg+1,pv+0);
00090   
00091     return result;
00092 }
00093 
00094 LSM303DLH::LSM303DLH(PinName sda, PinName scl):
00095     _compass(sda, scl), _offset_x(0), _offset_y(0), _offset_z(0), _scale_x(0), _scale_y(0), _scale_z(0), _filt_ax(0), _filt_ay(0), _filt_az(6000)
00096 {
00097     char reg_v;
00098     _compass.frequency(100000);
00099         
00100     reg_v = 0;
00101     reg_v |= 0x01 << 5;     /* Normal mode  */
00102     reg_v |= 0x07;          /* X/Y/Z axis enable. */
00103     write_reg(addr_acc,CTRL_REG1_A,reg_v);
00104     reg_v = 0;
00105     read_reg(addr_acc,CTRL_REG1_A,&reg_v);
00106 
00107     reg_v = 0;
00108     reg_v |= 0x01 << 6;     /* 1: data MSB @ lower address */
00109     reg_v |= 0x01 << 4;     /* +/- 4g */
00110     write_reg(addr_acc,CTRL_REG4_A,reg_v);
00111 
00112     /* -- mag --- */
00113     reg_v = 0;
00114     reg_v |= 0x04 << 2;     /* Minimum data output rate = 15Hz */
00115     write_reg(addr_mag,CRA_REG_M,reg_v);
00116 
00117     reg_v = 0;
00118     //reg_v |= 0x01 << 5;     /* +-1.3Gauss */
00119     reg_v |= 0x07 << 5;     /* +-8.1Gauss */
00120     write_reg(addr_mag,CRB_REG_M,reg_v);
00121 
00122     reg_v = 0;              /* Continuous-conversion mode */
00123     write_reg(addr_mag,MR_REG_M,reg_v);
00124 }
00125 
00126 
00127 void LSM303DLH::setOffset(float x, float y, float z)
00128 {
00129     _offset_x = x;
00130     _offset_y = y;
00131     _offset_z = z;   
00132 }
00133 
00134 void LSM303DLH::setScale(float x, float y, float z)
00135 {
00136     _scale_x = x;
00137     _scale_y = y;
00138     _scale_z = z;
00139 }
00140 
00141 void LSM303DLH::read(vector &a, vector &m)
00142 {
00143     short a_x, a_y, a_z;
00144     short m_x, m_y, m_z;
00145     //Timer t;
00146     //int usec1, usec2;
00147     
00148     //t.reset();
00149     //t.start();
00150 
00151     //usec1 = t.read_us();
00152     read_reg_short(addr_acc, OUT_X_A, &a_x);
00153     read_reg_short(addr_acc, OUT_Y_A, &a_y);
00154     read_reg_short(addr_acc, OUT_Z_A, &a_z);
00155     read_reg_short(addr_mag, OUT_X_M, &m_x);
00156     read_reg_short(addr_mag, OUT_Y_M, &m_y);
00157     read_reg_short(addr_mag, OUT_Z_M, &m_z);
00158     //usec2 = t.read_us();
00159     
00160     //if (debug) debug->printf("%d %d %d\n", usec1, usec2, usec2-usec1);
00161 
00162     // Perform simple lowpass filtering
00163     // Intended to stabilize heading despite
00164     // device vibration such as on a UGV
00165     _filt_ax += a_x - (_filt_ax >> FILTER_SHIFT);
00166     _filt_ay += a_y - (_filt_ay >> FILTER_SHIFT);
00167     _filt_az += a_z - (_filt_az >> FILTER_SHIFT);
00168 
00169     a.x = (float) (_filt_ax >> FILTER_SHIFT);
00170     a.y = (float) (_filt_ay >> FILTER_SHIFT);
00171     a.z = (float) (_filt_az >> FILTER_SHIFT);
00172     
00173     // offset and scale
00174     m.x = (m_x + _offset_x) * _scale_x;
00175     m.y = (m_y + _offset_y) * _scale_y;
00176     m.z = (m_z + _offset_z) * _scale_z;
00177 }
00178 
00179 
00180 // Returns the number of degrees from the -Y axis that it
00181 // is pointing.
00182 float LSM303DLH::heading()
00183 {
00184     return heading((vector){0,-1,0});
00185 }
00186 
00187 float LSM303DLH::heading(vector from)
00188 {
00189     vector a, m;
00190 
00191     this->read(a, m);
00192     
00193     ////////////////////////////////////////////////
00194     // compute heading       
00195     ////////////////////////////////////////////////
00196 
00197     vector temp_a = a;
00198     // normalize
00199     vector_normalize(&temp_a);
00200     //vector_normalize(&m);
00201 
00202     // compute E and N
00203     vector E;
00204     vector N;
00205     vector_cross(&m,&temp_a,&E);
00206     vector_normalize(&E);
00207     vector_cross(&temp_a,&E,&N);
00208     
00209     // compute heading
00210     float heading = atan2(vector_dot(&E,&from), vector_dot(&N,&from)) * 180/M_PI;
00211     if (heading < 0) heading += 360;
00212     
00213     return heading;
00214 }
00215 
00216 void LSM303DLH::frequency(int hz)
00217 {
00218     _compass.frequency(hz);
00219 }