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 |= 0x03 << 3;     /* Output data rate 1000 [Hz] */
00103     reg_v |= 0x07;          /* X/Y/Z axis enable. */
00104     write_reg(addr_acc,CTRL_REG1_A,reg_v);
00105     reg_v = 0;
00106     read_reg(addr_acc,CTRL_REG1_A,&reg_v);
00107 
00108     reg_v = 0;
00109     reg_v |= 0x01 << 6;     /* 1: data MSB @ lower address */
00110     reg_v |= 0x01 << 4;     /* +/- 4g */
00111     //reg_v |= 0x00 << 4;     /* +/- 2g */
00112     write_reg(addr_acc,CTRL_REG4_A,reg_v);
00113 
00114     /* -- mag --- */
00115     reg_v = 0;
00116     reg_v |= 0x04 << 2;     /* Minimum data output rate = 15Hz */
00117     write_reg(addr_mag,CRA_REG_M,reg_v);
00118 
00119     reg_v = 0;
00120     //reg_v |= 0x01 << 5;     /* +-1.3Gauss */
00121     reg_v |= 0x07 << 5;     /* +-8.1Gauss */
00122     write_reg(addr_mag,CRB_REG_M,reg_v);
00123 
00124     reg_v = 0;              /* Continuous-conversion mode */
00125     write_reg(addr_mag,MR_REG_M,reg_v);
00126 }
00127 
00128 
00129 void LSM303DLH::setOffset(float x, float y, float z)
00130 {
00131     _offset_x = x;
00132     _offset_y = y;
00133     _offset_z = z;   
00134 }
00135 
00136 void LSM303DLH::setScale(float x, float y, float z)
00137 {
00138     _scale_x = x;
00139     _scale_y = y;
00140     _scale_z = z;
00141 }
00142 
00143 void LSM303DLH::read(vector &a, vector &m)
00144 {
00145     short a_x, a_y, a_z;
00146     short m_x, m_y, m_z;
00147     //Timer t;
00148     //int usec1, usec2;
00149     
00150     //t.reset();
00151     //t.start();
00152 
00153     //usec1 = t.read_us();
00154     read_reg_short(addr_acc, OUT_X_A, &a_x);
00155     read_reg_short(addr_acc, OUT_Y_A, &a_y);
00156     read_reg_short(addr_acc, OUT_Z_A, &a_z);
00157     read_reg_short(addr_mag, OUT_X_M, &m_x);
00158     read_reg_short(addr_mag, OUT_Y_M, &m_y);
00159     read_reg_short(addr_mag, OUT_Z_M, &m_z);
00160     //usec2 = t.read_us();
00161     
00162     //if (debug) debug->printf("%d %d %d\n", usec1, usec2, usec2-usec1);
00163 
00164 #if 0
00165     // Perform simple lowpass filtering
00166     // Intended to stabilize heading despite
00167     // device vibration such as on a UGV
00168     _filt_ax += a_x - (_filt_ax >> FILTER_SHIFT);
00169     _filt_ay += a_y - (_filt_ay >> FILTER_SHIFT);
00170     _filt_az += a_z - (_filt_az >> FILTER_SHIFT);
00171 
00172     a.x = (float) (_filt_ax >> FILTER_SHIFT);
00173     a.y = (float) (_filt_ay >> FILTER_SHIFT);
00174     a.z = (float) (_filt_az >> FILTER_SHIFT);
00175 #else
00176     a.x = (float)a_x;
00177     a.y = (float)a_y;
00178     a.z = (float)a_z;
00179 #endif
00180     
00181     // offset and scale
00182     m.x = (m_x + _offset_x) * _scale_x;
00183     m.y = (m_y + _offset_y) * _scale_y;
00184     m.z = (m_z + _offset_z) * _scale_z;
00185 }
00186 
00187 
00188 // Returns the number of degrees from the -Y axis that it
00189 // is pointing.
00190 float LSM303DLH::heading()
00191 {
00192     return heading((vector){0,-1,0});
00193 }
00194 
00195 float LSM303DLH::heading(vector from)
00196 {
00197     vector a, m;
00198 
00199     this->read(a, m);
00200     
00201     ////////////////////////////////////////////////
00202     // compute heading       
00203     ////////////////////////////////////////////////
00204 
00205     vector temp_a = a;
00206     // normalize
00207     vector_normalize(&temp_a);
00208     //vector_normalize(&m);
00209 
00210     // compute E and N
00211     vector E;
00212     vector N;
00213     vector_cross(&m,&temp_a,&E);
00214     vector_normalize(&E);
00215     vector_cross(&temp_a,&E,&N);
00216     
00217     // compute heading
00218     float heading = atan2(vector_dot(&E,&from), vector_dot(&N,&from)) * 180/M_PI;
00219     if (heading < 0) heading += 360;
00220     
00221     return heading;
00222 }
00223 
00224 void LSM303DLH::frequency(int hz)
00225 {
00226     _compass.frequency(hz);
00227 }