LSM303D.cpp

00001 /** Tilt-compensated compass interface Library for the STMicro LSM303DLH 3-axis magnetometer, 3-axis acceleromter
00002  *
00003  * Written by Eric Coyle
00004  *
00005  * Based on Michael Shimniok's LSM303DLH library which is based on
00006  * test program by @tosihisa and 
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  */
00036  #include "LSM303D.h"
00037 #include "mbed.h"
00038  
00039 #ifndef M_PI
00040 #define M_PI 3.14159265358979323846
00041 #endif
00042  
00043 LSM303D::LSM303D(SPI &spi, PinName CS) : cs(CS), mspi(spi) {
00044     cs = 1;
00045 }
00046  
00047 int LSM303D::whoami() {
00048     int me;
00049     cs=0;   //talk to compass
00050     
00051     //Send who am I (first bit must be 1 for read)
00052     mspi.write(0x8F);
00053     
00054     //Get who am I response
00055     me = mspi.write(0x00);
00056     cs=1;   //done talking
00057     
00058     return me;
00059 }
00060 
00061 int LSM303D::initialize() {
00062     int iam;
00063     
00064     //Check device
00065     iam=whoami();
00066     
00067     if (iam==73) {
00068         
00069         //set up accelerometer
00070         //CTRL1
00071         cs=0;   //talk to compass
00072         mspi.write(0x20);
00073         mspi.write(0x67);  //100Hz, continuous update, all axes enabled
00074         cs=1;   //done talking
00075         
00076         
00077         //CTRL2
00078         cs=0;   //talk to compass
00079         mspi.write(0x21);
00080         mspi.write(0x00);  // 773Hz anti-alias filter, +/- 2g scale, self-test disabled, 4-wire SPI
00081         cs=1;   //done talking
00082         
00083         //CTRL3
00084         
00085         //set up magnetometer
00086         //CTRL5
00087         cs=0;   //talk to compass
00088         mspi.write(0x24);
00089         mspi.write(0x74);  //temperature disabled, high resolution, 100 Hz, int2 disable, int1 disabled
00090         cs=1;   //done talking
00091         
00092         
00093         //CTRL6 (Magnetic Scale)
00094         cs=0;   //talk to compass
00095         mspi.write(0x25);
00096         mspi.write(0x20);  //+/-4g
00097         cs=1;   //done talking
00098         
00099         //CTRL7 (filtering settings, and other)
00100         cs=0;   //talk to compass
00101         mspi.write(0x26);
00102         mspi.write(0x00); //normal mode, keep other on
00103         cs=1;   //done talking
00104         
00105         return 1;
00106     }
00107     else {
00108         //Not talking to right device
00109         return 0;
00110     }
00111 }
00112 
00113 void LSM303D::setOffset(float x, float y, float z)
00114 {
00115     _offset_x = x;
00116     _offset_y = y;
00117     _offset_z = z;   
00118 }
00119  
00120 void LSM303D::setScale(float x, float y, float z)
00121 {
00122     _scale_x = x;
00123     _scale_y = y;
00124     _scale_z = z;
00125 }
00126 
00127 int LSM303D::magnitometer(int axis) {
00128     if (axis==0) {
00129         cs=0;   //lower cs to talk
00130         mspi.write(0x88);
00131         compass.b[0] = mspi.write(0x00);
00132         
00133         cs=1;   //done talking
00134         cs=0;   //lower cs to talk
00135     
00136         mspi.write(0x89);
00137         compass.b[1] = mspi.write(0x00);
00138     
00139         cs=1;   //done talking
00140     }
00141     else if (axis==1) {
00142         //Y-Axis
00143         cs=0;
00144         mspi.write(0x8A);
00145         compass.b[0] = mspi.write(0x00);
00146         
00147         cs=1;   //done talking
00148         cs=0;   //lower cs to talk
00149         
00150         mspi.write(0x8B);
00151         compass.b[1] = mspi.write(0x00);
00152         
00153         cs=1;   //done talking
00154     }
00155     else if (axis==2) {
00156         //Z-Axis
00157         cs=0;
00158         mspi.write(0x8C);
00159         compass.b[0] = mspi.write(0x00);
00160         
00161         cs=1;   //done talking
00162         cs=0;   //lower cs to talk
00163         
00164         mspi.write(0x8D);
00165         compass.b[1] = mspi.write(0x00);
00166         
00167         cs=1;   //done talking
00168     }
00169     return compass.raw;
00170 }
00171 
00172 int LSM303D::accelerometer(int axis) {
00173     if (axis==0) {
00174         cs=0;   //lower cs to talk
00175         mspi.write(0xA8);
00176         compass.b[0] = mspi.write(0x00);
00177         
00178         cs=1;   //done talking
00179         cs=0;   //lower cs to talk
00180     
00181         mspi.write(0xA9);
00182         compass.b[1] = mspi.write(0x00);
00183     
00184         cs=1;   //done talking
00185     }
00186     else if (axis==1) {
00187         //Y-Axis
00188         cs=0;
00189         mspi.write(0xAA);
00190         compass.b[0] = mspi.write(0x00);
00191         
00192         cs=1;   //done talking
00193         cs=0;   //lower cs to talk
00194         
00195         mspi.write(0xAB);
00196         compass.b[1] = mspi.write(0x00);
00197         
00198         cs=1;   //done talking
00199     }
00200     else if (axis==2) {
00201         //Z-Axis
00202         cs=0;
00203         mspi.write(0xAC);
00204         compass.b[0] = mspi.write(0x00);
00205         
00206         cs=1;   //done talking
00207         cs=0;   //lower cs to talk
00208         
00209         mspi.write(0xAD);
00210         compass.b[1] = mspi.write(0x00);
00211         
00212         cs=1;   //done talking
00213     }
00214     return compass.raw;
00215 }
00216 
00217 void LSM303D::read(vector &a, vector &m)
00218 {
00219     short a_x, a_y, a_z;
00220     short m_x, m_y, m_z;
00221     //Timer t;
00222     //int usec1, usec2;
00223     
00224     //t.reset();
00225     //t.start();
00226  
00227     //usec1 = t.read_us();
00228     /*read_reg_short(addr_acc, OUT_X_A, &a_x);
00229     read_reg_short(addr_acc, OUT_Y_A, &a_y);
00230     read_reg_short(addr_acc, OUT_Z_A, &a_z);
00231     read_reg_short(addr_mag, OUT_X_M, &m_x);
00232     read_reg_short(addr_mag, OUT_Y_M, &m_y);
00233     read_reg_short(addr_mag, OUT_Z_M, &m_z);*/
00234     
00235     a_x=accelerometer(XAXIS);
00236     a_y=accelerometer(YAXIS);
00237     a_z=accelerometer(ZAXIS);
00238     m_x=magnitometer(XAXIS);
00239     m_y=magnitometer(YAXIS);
00240     m_z=magnitometer(ZAXIS);
00241     
00242     //usec2 = t.read_us();
00243     
00244     //if (debug) debug->printf("%d %d %d\n", usec1, usec2, usec2-usec1);
00245  
00246     // Perform simple lowpass filtering
00247     // Intended to stabilize heading despite
00248     // device vibration such as on a UGV
00249     _filt_ax += a_x - (_filt_ax >> FILTER_SHIFT);
00250     _filt_ay += a_y - (_filt_ay >> FILTER_SHIFT);
00251     _filt_az += a_z - (_filt_az >> FILTER_SHIFT);
00252  
00253     a.x = (float) (_filt_ax >> FILTER_SHIFT);
00254     a.y = (float) (_filt_ay >> FILTER_SHIFT);
00255     a.z = (float) (_filt_az >> FILTER_SHIFT);
00256     
00257     // offset and scale
00258     m.x = (m_x + _offset_x) * _scale_x;
00259     m.y = (m_y + _offset_y) * _scale_y;
00260     m.z = (m_z + _offset_z) * _scale_z;
00261 }
00262 
00263 /// Returns the number of degrees from the -Y axis that it
00264 // is pointing.
00265 float LSM303D::heading()
00266 {
00267     return heading((vector){0,-1,0});
00268 }
00269  
00270 float LSM303D::heading(vector from)
00271 {
00272     vector a, m;
00273  
00274     this->read(a, m);
00275     
00276     ////////////////////////////////////////////////
00277     // compute heading       
00278     ////////////////////////////////////////////////
00279  
00280     vector temp_a = a;
00281     // normalize
00282     vector_normalize(&temp_a);
00283     //vector_normalize(&m);
00284  
00285     // compute E and N
00286     vector E;
00287     vector N;
00288     vector_cross(&m,&temp_a,&E);
00289     vector_normalize(&E);
00290     vector_cross(&temp_a,&E,&N);
00291     
00292     // compute heading
00293     float heading = atan2(vector_dot(&E,&from), vector_dot(&N,&from)) * 180/M_PI;
00294     if (heading < 0) heading += 360;
00295     
00296     return heading;
00297 }