File content as of revision 2:85214374e094:

#include <LSM303.h>
#include "LSM303.h"
#include <math.h>

LSM303::LSM303( PinName sda = PTE0, PinName scl = PTE1 ) : i2c( sda, scl ) {
    // These are just some values for a particular unit; it is recommended that
    // a calibration be done for your particular unit.
    m_max.x = +540;
    m_max.y = +500;
    m_max.z = 180;
    m_min.x = -520;
    m_min.y = -570;
    m_min.z = -770;

    _device = LSM303DLM_DEVICE;
    acc_address = ACC_ADDRESS_SA0_A_LOW;
}

// Public Methods //////////////////////////////////////////////////////////////

void LSM303::init(byte device, byte sa0_a) {
    Serial pc(USBTX, USBRX);
    _device = device;
    switch (_device) {
        case LSM303DLH_DEVICE:
        case LSM303DLM_DEVICE:
            if (sa0_a == LSM303_SA0_A_LOW)
                acc_address = ACC_ADDRESS_SA0_A_LOW;
            else if (sa0_a == LSM303_SA0_A_HIGH)
                acc_address = ACC_ADDRESS_SA0_A_HIGH;
            else
                acc_address = (detectSA0_A() == LSM303_SA0_A_HIGH) ? ACC_ADDRESS_SA0_A_HIGH : ACC_ADDRESS_SA0_A_LOW;
            break;

        case LSM303DLHC_DEVICE:
            acc_address = ACC_ADDRESS_SA0_A_HIGH;
            break;

        default:
            // try to auto-detect device
            if (detectSA0_A() == LSM303_SA0_A_HIGH) {
                // if device responds on 0011001b (SA0_A is high), assume DLHC
                acc_address = ACC_ADDRESS_SA0_A_HIGH;
                _device = LSM303DLHC_DEVICE;
            } else {
                // otherwise, assume DLH or DLM (pulled low by default on Pololu boards); query magnetometer WHO_AM_I to differentiate these two
                acc_address = ACC_ADDRESS_SA0_A_LOW;
                _device = (readMagReg(LSM303_WHO_AM_I_M) == 0x3C) ?   LSM303DLM_DEVICE : LSM303DLH_DEVICE;
            }
    }
}

// Turns on the LSM303's accelerometer and magnetometers and places them in normal
// mode.
void LSM303::enableDefault(void) {
    // Enable Accelerometer
    // 0x27 = 0b00100111
    // Normal power mode, all axes enabled
    writeAccReg(LSM303_CTRL_REG1_A, 0x27);

    // Enable Magnetometer
    // 0x00 = 0b00000000
    // Continuous conversion mode
    writeMagReg(LSM303_MR_REG_M, 0x00);
}

// Writes an accelerometer register
void LSM303::writeAccReg(byte reg, byte value) {
    char data[2] = { reg, value };
    i2c.write( acc_address, data, 2 );
}

// Reads an accelerometer register
int LSM303::readAccReg(byte reg) {
    char value[1];
    char data[1] = { reg };
    i2c.write( acc_address, data, 1 );
    i2c.read( acc_address, value, 1 );
    return value[0];
}

// Writes a magnetometer register
void LSM303::writeMagReg(byte reg, byte value) {
    char data[2] = { reg, value };
    i2c.write( MAG_ADDRESS, data, 2 );
}

// Reads a magnetometer register
int LSM303::readMagReg(int reg) {
    char value[1];
    char data[1];
    // if dummy register address (magnetometer Y/Z), use device type to determine actual address
    if (reg < 0) {
        switch (reg) {
            case LSM303_OUT_Y_H_M:
                reg = (_device == LSM303DLH_DEVICE) ? LSM303DLH_OUT_Y_H_M : LSM303DLM_OUT_Y_H_M;
                break;
            case LSM303_OUT_Y_L_M:
                reg = (_device == LSM303DLH_DEVICE) ? LSM303DLH_OUT_Y_L_M : LSM303DLM_OUT_Y_L_M;
                break;
            case LSM303_OUT_Z_H_M:
                reg = (_device == LSM303DLH_DEVICE) ? LSM303DLH_OUT_Z_H_M : LSM303DLM_OUT_Z_H_M;
                break;
            case LSM303_OUT_Z_L_M:
                reg = (_device == LSM303DLH_DEVICE) ? LSM303DLH_OUT_Z_L_M : LSM303DLM_OUT_Z_L_M;
                break;
        }
    }
    data[0] = reg;
    i2c.write( MAG_ADDRESS, data, 1 );
    i2c.read( MAG_ADDRESS, value, 1 );
    return value[0];

}

// Reads the 3 accelerometer channels and stores them in vector a
void LSM303::readAcc(void) {
    char data[1] = { LSM303_OUT_X_L_A | (1<<7) };
    char out[6];
    i2c.write( acc_address, data, 1 );

    i2c.read( acc_address, out, 6 );

    a.x = short( ( out[1] << 8 | out[0] ) >> 4 );
    a.y = short( ( out[3] << 8 | out[2] ) >> 4 );
    a.z = short( ( out[5] << 8 | out[4] ) >> 4 );
}

// Reads the 3 magnetometer channels and stores them in vector m
void LSM303::readMag(void) {
    Serial pc(USBTX, USBRX);
    char data[1] = { LSM303_OUT_X_H_M };
    char out[6];
    
    i2c.write( MAG_ADDRESS, data, 1 );
    i2c.read( MAG_ADDRESS, out, 6 );

    if (_device == LSM303DLH_DEVICE) {
        // DLH: register address for Y comes before Z
        m.x = short( out[0] << 8 | out[1] );
        m.y = short( out[2] << 8 | out[3] );
        m.z = short( out[4] << 8 | out[5] );
    } else {
        // DLM, DLHC: register address for Z comes before Y
        m.x = short( out[0] << 8 | out[1] );
        m.y = short( out[4] << 8 | out[5] );
        m.z = short( out[2] << 8 | out[3] );
    }
}

// Reads all 6 channels of the LSM303 and stores them in the object variables
void LSM303::read(void) {
    readAcc();
    readMag();
}

// Returns the number of degrees from the -Y axis that it
// is pointing.
int LSM303::heading(void) {
    return heading((Plane) {
        0,-1,0
    });
}

// Returns the number of degrees from the From vector projected into
// the horizontal plane is away from north.
//
// Description of heading algorithm:
// Shift and scale the magnetic reading based on calibration data to
// to find the North vector. Use the acceleration readings to
// determine the Down vector. The cross product of North and Down
// vectors is East. The vectors East and North form a basis for the
// horizontal plane. The From vector is projected into the horizontal
// plane and the angle between the projected vector and north is
// returned.
int LSM303::heading(Plane from) {
    // shift and scale
    m.x = (m.x - m_min.x) / (m_max.x - m_min.x) * 2 - 1.0;
    m.y = (m.y - m_min.y) / (m_max.y - m_min.y) * 2 - 1.0;
    m.z = (m.z - m_min.z) / (m_max.z - m_min.z) * 2 - 1.0;

    Plane temp_a = a;
    // normalize
    vector_normalize(&temp_a);
    //vector_normalize(&m);

    // compute E and N
    Plane E;
    Plane N;
    vector_cross(&m, &temp_a, &E);
    vector_normalize(&E);
    vector_cross(&temp_a, &E, &N);

    // compute heading
    int heading = (int)(atan2(vector_dot(&E, &from), vector_dot(&N, &from)) * 180 / M_PI);
    if (heading < 0) heading += 360;
    return heading;
}

void LSM303::vector_cross( const Plane *a,const Plane *b, Plane *out ) {
    out->x = a->y*b->z - a->z*b->y;
    out->y = a->z*b->x - a->x*b->z;
    out->z = a->x*b->y - a->y*b->x;
}

float LSM303::vector_dot( const Plane *a,const Plane *b ) {
    return a->x*b->x+a->y*b->y+a->z*b->z;
}

void LSM303::vector_normalize( Plane *a ) {
    float mag = sqrt(vector_dot(a,a));
    a->x /= mag;
    a->y /= mag;
    a->z /= mag;
}


byte LSM303::detectSA0_A(void) {
    char out[1];
    char data[1] = { LSM303_CTRL_REG1_A };
    i2c.write( ACC_ADDRESS_SA0_A_LOW, data, 1 );
    
    if ( 0 == i2c.read( ACC_ADDRESS_SA0_A_LOW, out, 1 ) ) {
        return LSM303_SA0_A_LOW;
    } else {
        return LSM303_SA0_A_HIGH;
    }
}