Revision 0:4d358fbeab6e, committed 2013-10-10

Comitter:

fin4478

Date:

Thu Oct 10 06:57:53 2013 +0000

Child:

1:322c80f884d3

Commit message:

initial version

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LSM303.cpp	Thu Oct 10 06:57:53 2013 +0000
@@ -0,0 +1,159 @@
+#include "mbed.h"
+#include <math.h>
+#include "LSM303.h"
+
+I2C i2c(P0_5, P0_4);
+
+int LSM303::setup()
+{
+#if 1  
+    m_max.x = 239;
+    m_max.y = 242;
+    m_max.z = 158;
+    m_min.x = -637;
+    m_min.y = -498;
+    m_min.z = -538;
+#else    
+    m_max.x = 1;
+    m_max.y = 1;
+    m_max.z = 1;
+    m_min.x = 0;
+    m_min.y = 0;
+    m_min.z = 0;
+#endif
+    return initLSM303(ACCELE_SCALE);  // Initialize the LSM303, using a SCALE full-scale range
+}
+int LSM303::testAcc()
+{
+    if (i2c.write(LSM303_ACC, NULL, 0) ==0) return LSM303_ACC;
+    return 255;
+}
+
+int LSM303::testMag()
+{
+    if (i2c.write(LSM303_MAG, NULL, 0) ==0) 
+        if (LSM303_read(LSM303_WHO_AM_I_M)==0x3C) {
+            return LSM303_WHO_AM_I_M;
+        }
+        else {
+            return LSM303_MAG;
+        }
+    
+    return 255;
+}
+
+int LSM303::initLSM303(int fs)
+{
+    if (LSM303_write(0x27, CTRL_REG1_A) == 1) {  // 0x27 = normal power mode, all accel axes on
+        //LSM303_write(16, CTRL_REG2_A); // enable internal hp filter
+    }
+    if ((fs==8)||(fs==4))
+        LSM303_write((0x00 | (fs-fs/2-1)<<4), CTRL_REG4_A);  // set full-scale
+    else
+        LSM303_write(0x00, CTRL_REG4_A);
+    //LSM303_write(20, CRA_REG_M);  // 20 = mag 30Hz output rate
+    LSM303_write(MAG_SCALE_1_3, CRB_REG_M); //magnetic scale = +/-1.3Gauss
+    LSM303_write(0x00, MR_REG_M);  // 0x00 = continouous conversion mode
+
+    return 1;
+}
+
+float LSM303::getTiltHeading()
+{
+    //shift and scale
+    a.x = a.x / 32768 * ACCELE_SCALE;
+    a.y = a.y / 32768 * ACCELE_SCALE;
+    a.z = a.z / 32768 * ACCELE_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;
+    
+    vector_normalize(&a);
+    vector_normalize(&m);
+    //see appendix A in app note AN3192
+    pitch = asin(-a.x);
+    roll = asin(a.y/cos(pitch));
+    float heading = 0;
+    float xh = m.x * cos(pitch) + m.z * sin(pitch);
+    float yh = m.x * sin(roll) * sin(pitch) + m.y * cos(roll) - m.z * sin(roll) * cos(pitch);
+    //float zh = -m.x * cos(roll) * sin(pitch) + m.y * sin(roll) + m.z * cos(roll) * cos(pitch);      
+    heading = 180 * atan2(yh, xh)/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;
+}
+
+int LSM303::getLSM303_accel()
+{
+    char data[1] = { OUT_X_L_A | (1<<7)};
+    char out[6] = {0,0,0,0,0,0};
+    i2c.write( LSM303_ACC, data,1);
+    i2c.read( LSM303_ACC, out, 6);
+
+    a.x = short( (((short)out[1]) << 8) | out[0] );
+    a.y = short( (((short)out[3]) << 8) | out[2] );
+    a.z = short( (((short)out[5]) << 8) | out[4] );
+    return  0;
+}
+
+void LSM303::getLSM303_mag()
+{
+    char data[1] = { OUT_X_H_M };
+    char out[6];
+
+    i2c.write( LSM303_MAG, data, 1 );
+    i2c.read( LSM303_MAG, out, 6 );
+    // 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] );
+}
+
+int LSM303::LSM303_read(int address)
+{
+    if (address >= 0x20) {
+        _i2c_address = LSM303_ACC;
+    } else {
+        _i2c_address = LSM303_MAG;
+    }
+
+    char value[1];
+
+    char data[1] = { address };
+    i2c.write( _i2c_address, data, 1 );
+    i2c.read( _i2c_address, value, 1 );
+    return value[0];
+}
+
+int LSM303::LSM303_write(int data, int address)
+{
+    if (address >= 0x20) {
+        _i2c_address = LSM303_ACC;
+    } else {
+        _i2c_address = LSM303_MAG;
+    }
+
+    char out[2] = { address, data };
+    i2c.write( _i2c_address, out, 2 );
+    return 0;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LSM303.h	Thu Oct 10 06:57:53 2013 +0000
@@ -0,0 +1,144 @@
+#ifndef LSM303_h
+#define LSM303_h
+#include "mbed.h"
+/* LSM303DLM Example Code base on LSM303DLH example code by Jim Lindblom SparkFun Electronics
+
+   date: 9/6/11
+   license: Creative commons share-alike v3.0
+
+   Modified by:Frankie.Chu
+
+   Summary:
+   Show how to calculate level and tilt-compensated heading using
+   the snazzy LSM303DLH 3-axis magnetometer/3-axis accelerometer.
+
+   Firmware:
+   You can set the accelerometer's full-scale range by setting
+   the SCALE constant to either 2, 4, or 8. This value is used
+   in the initLSM303() function. For the most part, all other
+   registers in the LSM303 will be at their default value.
+
+   Use the LSM303_write() and LSM303_read() functions to write
+   to and read from the LSM303's internal registers.
+
+   Use getLSM303_accel() and getLSM303_mag() to get the acceleration
+   and magneto values from the LSM303. You'll need to pass each of
+   those functions an array, where the data will be stored upon
+   return from the void.
+
+   getHeading() calculates a heading assuming the sensor is level.
+   A float between 0 and 360 is returned. You need to pass it a
+   array with magneto values.
+
+   getTiltHeading() calculates a tilt-compensated heading.
+   A float between 0 and 360 degrees is returned. You need
+   to pass this function both a magneto and acceleration array.
+
+   Headings are calculated as specified in AN3192:
+   http://www.sparkfun.com/datasheets/Sensors/Magneto/Tilt%20Compensated%20Compass.pdf
+
+   Hardware:
+   I'm using SparkFun's LSM303 breakout. Only power and the two
+   I2C lines are connected:
+   LSM303 Breakout ---------- Arduino
+         Vin                   5V
+         GND                   GND
+         SDA                   A4
+         SCL                   A5
+*/
+
+
+
+#define ACCELE_SCALE 2  // accelerometer full-scale, should be 2, 4, or 8
+
+/* LSM303 Address definitions */
+#define LSM303_MAG  0x3C  // assuming SA0 grounded
+#define LSM303_ACC  0x30  // assuming SA0 grounded
+
+#define X 0
+#define Y 1
+#define Z 2
+
+/* LSM303 Register definitions */
+#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 HP_FILTER_RESET_A 0x25
+#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_SOURCE_A 0x31
+#define INT1_THS_A 0x32
+#define INT1_DURATION_A 0x33
+#define CRA_REG_M 0x00
+#define CRB_REG_M 0x01//refer to the Table 58 of the datasheet of LSM303DLM
+#define MAG_SCALE_1_3 0x20//full-scale is +/-1.3Gauss
+#define MAG_SCALE_1_9 0x40//+/-1.9Gauss
+#define MAG_SCALE_2_5 0x60//+/-2.5Gauss
+#define MAG_SCALE_4_0 0x80//+/-4.0Gauss
+#define MAG_SCALE_4_7 0xa0//+/-4.7Gauss
+#define MAG_SCALE_5_6 0xc0//+/-5.6Gauss
+#define MAG_SCALE_8_1 0xe0//+/-8.1Gauss
+#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 LSM303_WHO_AM_I_M        0x0F // DLM only
+
+#define PI                    3.14159265
+
+class LSM303
+{// I am  LSM303DLM
+public:
+    typedef struct Plane {
+        float x, y, z;
+    } Plane;
+    
+    Plane a; // accelerometer readings
+    Plane m; // magnetometer readings
+    Plane m_max; // maximum magnetometer values, used for calibration
+    Plane m_min; // minimum magnetometer values, used for calibration
+    float pitch;
+    float roll;
+    int setup();
+    
+    void getLSM303_mag();
+    int getLSM303_accel();
+    int LSM303_read(int address);
+    int LSM303_write(int data, int address);
+    
+    int testAcc();
+    int testMag();
+    
+    float getTiltHeading();
+    
+    // Plane functions
+    static void vector_cross(const Plane *a, const Plane *b, Plane *out);
+    static float vector_dot(const Plane *a,const Plane *b);
+    static void vector_normalize(Plane *a);
+
+
+private:
+ 
+    int _i2c_address;
+
+    int initLSM303(int fs); // accelerometer full-scale, should be 2, 4, or 8
+};
+
+#endif
+