LSM303DLM.cpp

00001 #include "mbed.h"
00002 #include "LSM303DLM.h"
00003 #include "stdio.h"
00004 
00005 #define MAG_ADDRESS  0x3C
00006 #define ACC_ADDRESS  0x30
00007 
00008 LSM303DLM::LSM303DLM(PinName sda, PinName scl): _device(sda, scl)
00009 {
00010     _device.frequency(400000);
00011     init();
00012 }
00013 
00014 void LSM303DLM::init()
00015 {
00016     // init mag
00017     // continuous conversion mode
00018     _data[0] = MR_REG_M;
00019     _data[1] = 0x00;
00020     _device.write(MAG_ADDRESS, _data, 2);
00021     // data rate 75hz
00022     _data[0] = CRA_REG_M;
00023     _data[1] = 0x18; // 0b00011000
00024     _device.write(MAG_ADDRESS, _data, 2);
00025     // init acc
00026     // data rate 100hz 
00027     _data[0] = CTRL_REG1_A;
00028     _data[1] = 0x2F; // 0b00101111
00029     _device.write(ACC_ADDRESS, _data, 2);
00030 }
00031 
00032 void LSM303DLM::read(int a[3], int m[3])
00033 {
00034     readAcc(a);
00035     readMag(m);   
00036 }
00037 
00038 void LSM303DLM::readAcc(int a[3])
00039 {
00040     _data[0] = OUT_X_L_A | (1<<7);
00041     _device.write(ACC_ADDRESS, _data, 1);
00042     _device.read(ACC_ADDRESS, _data, 6);
00043 
00044     // 12-bit values
00045     a[0] = (short) (_data[1]<<8 | _data[0]) >> 4;
00046     a[1] = (short) (_data[3]<<8 | _data[2]) >> 4;
00047     a[2] = (short) (_data[5]<<8 | _data[4]) >> 4;
00048 }
00049 
00050 void LSM303DLM::readMag(int m[3])
00051 {
00052     _data[0] = OUT_X_H_M;
00053     _device.write(MAG_ADDRESS, _data, 1);
00054     _device.read(MAG_ADDRESS, _data, 6);
00055     
00056     m[0] = (short) (_data[0]<<8 | _data[1]); // X
00057     m[1] = (short) (_data[4]<<8 | _data[5]); // Y
00058     m[2] = (short) (_data[2]<<8 | _data[3]); // Z
00059  }
00060 
00061 void LSM303DLM::setScale(float x, float y, float z)
00062 {
00063     scale[0] = x;
00064     scale[1] = y;
00065     scale[2] = z;
00066 }
00067 
00068 void LSM303DLM::setOffset(float x, float y, float z)
00069 {
00070     offset[0] = x;
00071     offset[1] = y;
00072     offset[2] = z;
00073 }