Revision 0:faef9e4c8bea, committed 2012-01-20

Comitter:

shimniok

Date:

Fri Jan 20 23:47:25 2012 +0000

Child:

1:fc5c9258ec45

Commit message:

Initial version

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LSM303DLM.cpp	Fri Jan 20 23:47:25 2012 +0000
@@ -0,0 +1,63 @@
+#include "mbed.h"
+#include "LSM303DLM.h"
+#include "stdio.h"
+
+#define MAG_ADDRESS  0x3C
+#define ACC_ADDRESS  0x30
+
+LSM303DLM::LSM303DLM(PinName sda, PinName scl): _device(sda, scl)
+{
+    _device.frequency(400000);
+    init();
+}
+
+void LSM303DLM::init()
+{
+    // init mag
+    // continuous conversion mode
+    _data[0] = MR_REG_M;
+    _data[1] = 0x00;
+    if (!_device.write(MAG_ADDRESS, _data, 2))
+        fprintf(stdout, "MR_REG_M: no ack\n");
+    // data rate 75hz
+    _data[0] = CRA_REG_M;
+    _data[1] = 0x18; // 0b00011000
+    if (_device.write(MAG_ADDRESS, _data, 2))
+        fprintf(stdout, "CRA_REG_M: no ack\n");
+    
+    // init acc
+    // data rate 100hz 
+    _data[0] = CTRL_REG1_A;
+    _data[1] = 0x2F; // 0b00101111
+    if (!_device.write(ACC_ADDRESS, _data, 2))
+        fprintf(stdout, "CTRL_REG1_A: no ack\n");
+}
+
+void LSM303DLM::read(int a[3], int m[3])
+{
+    readAcc(a);
+    readMag(m);   
+}
+
+void LSM303DLM::readAcc(int a[3])
+{
+    _data[0] = OUT_X_L_A | (1<<7);
+    _device.write(ACC_ADDRESS, _data, 1);
+    _device.read(ACC_ADDRESS, _data, 6);
+
+    // 12-bit values
+    a[0] = (short) (_data[0] | _data[1]<<8)>>4; 
+    a[1] = (short) (_data[2] | _data[3]<<8)>>4;
+    a[2] = (short) (_data[4] | _data[5]<<8)>>4;
+}
+
+void LSM303DLM::readMag(int m[3])
+{
+    _data[0] = OUT_X_H_M;
+    _device.write(MAG_ADDRESS, _data, 1);
+    _device.read(MAG_ADDRESS, _data, 6);
+    
+    m[0] = (short) _data[0]<<8 | _data[1]; // X
+    m[1] = (short) _data[4]<<8 | _data[5]; // Y
+    m[2] = (short) _data[2]<<8 | _data[3]; // Z
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LSM303DLM.h	Fri Jan 20 23:47:25 2012 +0000
@@ -0,0 +1,130 @@
+#ifndef __LSM303DLM_H
+#define __LSM303DLM_H
+
+#include "mbed.h"
+
+// register addresses
+
+#define CTRL_REG1_A       0x20
+#define CTRL_REG2_A       0x21
+#define CTRL_REG3_A       0x22
+#define CTRL_REG4_A       0x23
+#define CTRL_REG5_A       0x24
+#define CTRL_REG6_A       0x25 // DLHC only
+#define HP_FILTER_RESET_A 0x25 // DLH, DLM only
+#define REFERENCE_A       0x26
+#define STATUS_REG_A      0x27
+
+#define OUT_X_L_A         0x28
+#define OUT_X_H_A         0x29
+#define OUT_Y_L_A         0x2A
+#define OUT_Y_H_A         0x2B
+#define OUT_Z_L_A         0x2C
+#define OUT_Z_H_A         0x2D
+
+#define INT1_CFG_A        0x30
+#define INT1_SRC_A        0x31
+#define INT1_THS_A        0x32
+#define INT1_DURATION_A   0x33
+#define INT2_CFG_A        0x34
+#define INT2_SRC_A        0x35
+#define INT2_THS_A        0x36
+#define INT2_DURATION_A   0x37
+
+#define CRA_REG_M         0x00
+#define CRB_REG_M         0x01
+#define MR_REG_M          0x02
+
+#define OUT_X_H_M         0x03
+#define OUT_X_L_M         0x04
+#define OUT_Y_H_M         0x07
+#define OUT_Y_L_M         0x08
+#define OUT_Z_H_M         0x05
+#define OUT_Z_L_M         0x06
+
+#define SR_REG_M          0x09
+#define IRA_REG_M         0x0A
+#define IRB_REG_M         0x0B
+#define IRC_REG_M         0x0C
+
+#define WHO_AM_I_M        0x0F
+
+#define OUT_Z_H_M         0x05
+#define OUT_Z_L_M         0x06
+#define OUT_Y_H_M         0x07
+#define OUT_Y_L_M         0x08
+
+/** Tilt-compensated compass interface Library for the STMicro LSM303DLm 3-axis magnetometer, 3-axis acceleromter
+ * @author Michael Shimniok http://www.bot-thoughts.com/
+ */
+class LSM303DLM {
+
+    public:
+        /** Create a new interface for an LSM303DLM
+         *
+         * @param sda is the pin for the I2C SDA line
+         * @param scl is the pin for the I2C SCL line
+         */
+        LSM303DLM(PinName sda, PinName scl);
+
+        /** sets the x, y, and z offset corrections for hard iron calibration
+         * 
+         * Calibration details here:
+         *  http://mbed.org/users/shimniok/notebook/quick-and-dirty-3d-compass-calibration/
+         *
+         * If you gather raw magnetometer data and find, for example, x is offset
+         * by hard iron by -20 then pass +20 to this member function to correct
+         * for hard iron.
+         *
+         * @param x is the offset correction for the x axis
+         * @param y is the offset correction for the y axis
+         * @param z is the offset correction for the z axis
+         */
+        void setOffset(float x, float y, float z);
+        
+        /** sets the scale factor for the x, y, and z axes
+         *
+         * Calibratio details here:
+         *  http://mbed.org/users/shimniok/notebook/quick-and-dirty-3d-compass-calibration/
+         *
+         * Sensitivity of the three axes is never perfectly identical and this
+         * function can help to correct differences in sensitivity.  You're
+         * supplying a multipler such that x, y and z will be normalized to the
+         * same max/min values
+         */
+        void setScale(float x, float y, float z);
+
+        /** read the calibrated accelerometer and magnetometer values
+         *
+         * @param a is the accelerometer 3d vector, written by the function
+         * @param m is the magnetometer 3d vector, written by the function
+         */
+        void read(int a[3], int m[3]);
+        
+        /** read the calibrated accelerometer values
+         *
+         * @param a is the accelerometer 3d vector, written by the function
+         */
+        void readAcc(int a[3]);
+
+        /** read the calibrated magnetometer values
+         *
+         * @param m is the magnetometer 3d vector, written by the function
+         */
+        void readMag(int m[3]);
+
+        /** sets the I2C bus frequency
+         *
+         * @param frequency is the I2C bus/clock frequency, either standard (100000) or fast (400000)
+         */
+        void frequency(int hz);
+     
+    private:
+        I2C _device;
+        char _data[6];
+        /** initializes the device
+         */
+        void init();
+};
+
+#endif
\ No newline at end of file