File content as of revision 2:171076b97de0:

/** Tilt-compensated compass interface Library for the STMicro LSM303DLH 3-axis magnetometer, 3-axis acceleromter
 *
 * Written by Eric Coyle
 *
 * Based on Michael Shimniok's LSM303DLH library which is based on
 * test program by @tosihisa and 
 * Pololu sample library for LSM303DLH breakout by ryantm:
 *
 * Copyright (c) 2011 Pololu Corporation. For more information, see
 *
 * http://www.pololu.com/
 * http://forum.pololu.com/
 *
 * Permission is hereby granted, free of charge, to any person
 * obtaining a copy of this software and associated documentation
 * files (the "Software"), to deal in the Software without
 * restriction, including without limitation the rights to use,
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following
 * conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 * 
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 * OTHER DEALINGS IN THE SOFTWARE.
 *
 */
 #include "LSM303D.h"
#include "mbed.h"
 
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
 
LSM303D::LSM303D(SPI &spi, PinName CS) : cs(CS), mspi(spi) {
    cs = 1;
}
 
int LSM303D::whoami() {
    int me;
    cs=0;   //talk to compass
    
    //Send who am I (first bit must be 1 for read)
    mspi.write(0x8F);
    
    //Get who am I response
    me = mspi.write(0x00);
    cs=1;   //done talking
    
    return me;
}

int LSM303D::initialize() {
    int iam;
    
    //Check device
    iam=whoami();
    
    if (iam==73) {
        
        //set up accelerometer
        //CTRL1
        cs=0;   //talk to compass
        mspi.write(0x20);
        mspi.write(0x67);  //100Hz, continuous update, all axes enabled
        cs=1;   //done talking
        
        
        //CTRL2
        cs=0;   //talk to compass
        mspi.write(0x21);
        mspi.write(0x00);  // 773Hz anti-alias filter, +/- 2g scale, self-test disabled, 4-wire SPI
        cs=1;   //done talking
        
        //CTRL3
        
        //set up magnetometer
        //CTRL5
        cs=0;   //talk to compass
        mspi.write(0x24);
        mspi.write(0x74);  //temperature disabled, high resolution, 100 Hz, int2 disable, int1 disabled
        cs=1;   //done talking
        
        
        //CTRL6 (Magnetic Scale)
        cs=0;   //talk to compass
        mspi.write(0x25);
        mspi.write(0x20);  //+/-4g
        cs=1;   //done talking
        
        //CTRL7 (filtering settings, and other)
        cs=0;   //talk to compass
        mspi.write(0x26);
        mspi.write(0x00); //normal mode, keep other on
        cs=1;   //done talking
        
        return 1;
    }
    else {
        //Not talking to right device
        return 0;
    }
}

void LSM303D::setOffset(float x, float y, float z)
{
    _offset_x = x;
    _offset_y = y;
    _offset_z = z;   
}
 
void LSM303D::setScale(float x, float y, float z)
{
    _scale_x = x;
    _scale_y = y;
    _scale_z = z;
}

int LSM303D::magnitometer(int axis) {
    if (axis==0) {
        cs=0;   //lower cs to talk
        mspi.write(0x88);
        compass.b[0] = mspi.write(0x00);
        
        cs=1;   //done talking
        cs=0;   //lower cs to talk
    
        mspi.write(0x89);
        compass.b[1] = mspi.write(0x00);
    
        cs=1;   //done talking
    }
    else if (axis==1) {
        //Y-Axis
        cs=0;
        mspi.write(0x8A);
        compass.b[0] = mspi.write(0x00);
        
        cs=1;   //done talking
        cs=0;   //lower cs to talk
        
        mspi.write(0x8B);
        compass.b[1] = mspi.write(0x00);
        
        cs=1;   //done talking
    }
    else if (axis==2) {
        //Z-Axis
        cs=0;
        mspi.write(0x8C);
        compass.b[0] = mspi.write(0x00);
        
        cs=1;   //done talking
        cs=0;   //lower cs to talk
        
        mspi.write(0x8D);
        compass.b[1] = mspi.write(0x00);
        
        cs=1;   //done talking
    }
    return compass.raw;
}

int LSM303D::accelerometer(int axis) {
    if (axis==0) {
        cs=0;   //lower cs to talk
        mspi.write(0xA8);
        compass.b[0] = mspi.write(0x00);
        
        cs=1;   //done talking
        cs=0;   //lower cs to talk
    
        mspi.write(0xA9);
        compass.b[1] = mspi.write(0x00);
    
        cs=1;   //done talking
    }
    else if (axis==1) {
        //Y-Axis
        cs=0;
        mspi.write(0xAA);
        compass.b[0] = mspi.write(0x00);
        
        cs=1;   //done talking
        cs=0;   //lower cs to talk
        
        mspi.write(0xAB);
        compass.b[1] = mspi.write(0x00);
        
        cs=1;   //done talking
    }
    else if (axis==2) {
        //Z-Axis
        cs=0;
        mspi.write(0xAC);
        compass.b[0] = mspi.write(0x00);
        
        cs=1;   //done talking
        cs=0;   //lower cs to talk
        
        mspi.write(0xAD);
        compass.b[1] = mspi.write(0x00);
        
        cs=1;   //done talking
    }
    return compass.raw;
}

void LSM303D::read(vector &a, vector &m)
{
    short a_x, a_y, a_z;
    short m_x, m_y, m_z;
    //Timer t;
    //int usec1, usec2;
    
    //t.reset();
    //t.start();
 
    //usec1 = t.read_us();
    /*read_reg_short(addr_acc, OUT_X_A, &a_x);
    read_reg_short(addr_acc, OUT_Y_A, &a_y);
    read_reg_short(addr_acc, OUT_Z_A, &a_z);
    read_reg_short(addr_mag, OUT_X_M, &m_x);
    read_reg_short(addr_mag, OUT_Y_M, &m_y);
    read_reg_short(addr_mag, OUT_Z_M, &m_z);*/
    
    a_x=accelerometer(XAXIS);
    a_y=accelerometer(YAXIS);
    a_z=accelerometer(ZAXIS);
    m_x=magnitometer(XAXIS);
    m_y=magnitometer(YAXIS);
    m_z=magnitometer(ZAXIS);
    
    //usec2 = t.read_us();
    
    //if (debug) debug->printf("%d %d %d\n", usec1, usec2, usec2-usec1);
 
    // Perform simple lowpass filtering
    // Intended to stabilize heading despite
    // device vibration such as on a UGV
    _filt_ax += a_x - (_filt_ax >> FILTER_SHIFT);
    _filt_ay += a_y - (_filt_ay >> FILTER_SHIFT);
    _filt_az += a_z - (_filt_az >> FILTER_SHIFT);
 
    a.x = (float) (_filt_ax >> FILTER_SHIFT);
    a.y = (float) (_filt_ay >> FILTER_SHIFT);
    a.z = (float) (_filt_az >> FILTER_SHIFT);
    
    // offset and scale
    m.x = (m_x + _offset_x) * _scale_x;
    m.y = (m_y + _offset_y) * _scale_y;
    m.z = (m_z + _offset_z) * _scale_z;
}

/// Returns the number of degrees from the -Y axis that it
// is pointing.
float LSM303D::heading()
{
    return heading((vector){0,-1,0});
}
 
float LSM303D::heading(vector from)
{
    vector a, m;
 
    this->read(a, m);
    
    ////////////////////////////////////////////////
    // compute heading       
    ////////////////////////////////////////////////
 
    vector temp_a = a;
    // normalize
    vector_normalize(&temp_a);
    //vector_normalize(&m);
 
    // compute E and N
    vector E;
    vector N;
    vector_cross(&m,&temp_a,&E);
    vector_normalize(&E);
    vector_cross(&temp_a,&E,&N);
    
    // compute heading
    float heading = atan2(vector_dot(&E,&from), vector_dot(&N,&from)) * 180/M_PI;
    if (heading < 0) heading += 360;
    
    return heading;
}