File content as of revision 23:9be034083282:

#include "LSM303DLHC.h"

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

LSM303DLHC::LSM303DLHC(I2C *obj) : i2c(obj)
{
    write(ACCEL_SAD, CTRL_REG1_A, 0x57);

    write(MAGNET_SAD, CRA_REG_M, 0x90);
    write(MAGNET_SAD, CRB_REG_M, 0x20);
    write(MAGNET_SAD, MR_REG_M, 0x00);
}

float LSM303DLHC::orientation(void)
{
        /* 加速度センサ未使用版
        float ans = atan2(float(magneticY()), float(magneticX()));
        if (ans < 0) ans += 2*M_PI ;
        if (ans > 2*M_PI) ans -= 2*M_PI;
        ans = ans * 180/M_PI ;
        if (ans > 360.0) ans = ans - 360.0 ;
        return ans ;
        */
        //加速度センサ使用版
        return heading((vector){0,-1,0});
}

float LSM303DLHC::temperature(void)
{
    char data[2];
    read(MAGNET_SAD, TEMP_OUT_M, data, 2);
    //マルツにあるサンプルをみたら25を足している
    //データシートにはそんな記述はないが
    //確かにそれで、それらしい値になる
    return ((data[0]<<8)|data[1])/256.0+25;
}

void LSM303DLHC::write(char sad, char reg, char data)
{
    char rw[2];
    rw[0] = reg;
    rw[1] = data;
    i2c->write(sad, rw, 2);
}

void LSM303DLHC::read(char sad, char reg, char *data, int length)
{
    reg |= 0x80; //MSB of register address means auto increment mode
    i2c->write(sad, &reg, 1);
    i2c->read(sad, data, length);
}


void LSM303DLHC::vector_cross(const vector *a,const vector *b, vector *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 LSM303DLHC::vector_dot(const vector *a,const vector *b)
{
  return a->x*b->x+a->y*b->y+a->z*b->z;
}

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

float LSM303DLHC::heading(vector from)
{
    vector a, m;
    a.x = accelX();
    a.y = accelY();
    a.z = accelZ();
    m.x = magnetX();
    m.y = magnetY();
    m.z = magnetZ();
    
    ////////////////////////////////////////////////
    // 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;
}