Compass Developer / LSM303DM

library for using LSM303DM chip

LSM303.cpp@1:322c80f884d3, 2013-10-11 (annotated)

Committer:
fin4478
Date:
Fri Oct 11 12:51:54 2013 +0000
Revision:
1:322c80f884d3
Parent:
0:4d358fbeab6e
Child:
2:1052b1b97cc2
ok calibration values

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fin4478 0:4d358fbeab6e 1 #include "mbed.h"
fin4478 0:4d358fbeab6e 2 #include <math.h>
fin4478 0:4d358fbeab6e 3 #include "LSM303.h"
fin4478 0:4d358fbeab6e 4
fin4478 0:4d358fbeab6e 5 I2C i2c(P0_5, P0_4);
fin4478 0:4d358fbeab6e 6
fin4478 0:4d358fbeab6e 7 int LSM303::setup()
fin4478 0:4d358fbeab6e 8 {
fin4478 1:322c80f884d3 9 #if 1
fin4478 1:322c80f884d3 10 m_max.x = 242;
fin4478 0:4d358fbeab6e 11 m_max.y = 242;
fin4478 0:4d358fbeab6e 12 m_max.z = 158;
fin4478 1:322c80f884d3 13 m_min.x = -498;
fin4478 0:4d358fbeab6e 14 m_min.y = -498;
fin4478 0:4d358fbeab6e 15 m_min.z = -538;
fin4478 1:322c80f884d3 16 #else
fin4478 0:4d358fbeab6e 17 m_max.x = 1;
fin4478 0:4d358fbeab6e 18 m_max.y = 1;
fin4478 0:4d358fbeab6e 19 m_max.z = 1;
fin4478 0:4d358fbeab6e 20 m_min.x = 0;
fin4478 0:4d358fbeab6e 21 m_min.y = 0;
fin4478 0:4d358fbeab6e 22 m_min.z = 0;
fin4478 0:4d358fbeab6e 23 #endif
fin4478 0:4d358fbeab6e 24 return initLSM303(ACCELE_SCALE); // Initialize the LSM303, using a SCALE full-scale range
fin4478 0:4d358fbeab6e 25 }
fin4478 0:4d358fbeab6e 26 int LSM303::testAcc()
fin4478 0:4d358fbeab6e 27 {
fin4478 0:4d358fbeab6e 28 if (i2c.write(LSM303_ACC, NULL, 0) ==0) return LSM303_ACC;
fin4478 0:4d358fbeab6e 29 return 255;
fin4478 0:4d358fbeab6e 30 }
fin4478 0:4d358fbeab6e 31
fin4478 0:4d358fbeab6e 32 int LSM303::testMag()
fin4478 0:4d358fbeab6e 33 {
fin4478 1:322c80f884d3 34 if (i2c.write(LSM303_MAG, NULL, 0) ==0)
fin4478 0:4d358fbeab6e 35 if (LSM303_read(LSM303_WHO_AM_I_M)==0x3C) {
fin4478 0:4d358fbeab6e 36 return LSM303_WHO_AM_I_M;
fin4478 1:322c80f884d3 37 } else {
fin4478 0:4d358fbeab6e 38 return LSM303_MAG;
fin4478 0:4d358fbeab6e 39 }
fin4478 1:322c80f884d3 40
fin4478 0:4d358fbeab6e 41 return 255;
fin4478 0:4d358fbeab6e 42 }
fin4478 0:4d358fbeab6e 43
fin4478 0:4d358fbeab6e 44 int LSM303::initLSM303(int fs)
fin4478 0:4d358fbeab6e 45 {
fin4478 0:4d358fbeab6e 46 if (LSM303_write(0x27, CTRL_REG1_A) == 1) { // 0x27 = normal power mode, all accel axes on
fin4478 0:4d358fbeab6e 47 //LSM303_write(16, CTRL_REG2_A); // enable internal hp filter
fin4478 0:4d358fbeab6e 48 }
fin4478 0:4d358fbeab6e 49 if ((fs==8)||(fs==4))
fin4478 0:4d358fbeab6e 50 LSM303_write((0x00 | (fs-fs/2-1)<<4), CTRL_REG4_A); // set full-scale
fin4478 0:4d358fbeab6e 51 else
fin4478 0:4d358fbeab6e 52 LSM303_write(0x00, CTRL_REG4_A);
fin4478 0:4d358fbeab6e 53 //LSM303_write(20, CRA_REG_M); // 20 = mag 30Hz output rate
fin4478 0:4d358fbeab6e 54 LSM303_write(MAG_SCALE_1_3, CRB_REG_M); //magnetic scale = +/-1.3Gauss
fin4478 0:4d358fbeab6e 55 LSM303_write(0x00, MR_REG_M); // 0x00 = continouous conversion mode
fin4478 0:4d358fbeab6e 56
fin4478 0:4d358fbeab6e 57 return 1;
fin4478 0:4d358fbeab6e 58 }
fin4478 0:4d358fbeab6e 59
fin4478 0:4d358fbeab6e 60 float LSM303::getTiltHeading()
fin4478 0:4d358fbeab6e 61 {
fin4478 0:4d358fbeab6e 62 //shift and scale
fin4478 0:4d358fbeab6e 63 a.x = a.x / 32768 * ACCELE_SCALE;
fin4478 0:4d358fbeab6e 64 a.y = a.y / 32768 * ACCELE_SCALE;
fin4478 1:322c80f884d3 65 a.z = a.z + 950;
fin4478 0:4d358fbeab6e 66 a.z = a.z / 32768 * ACCELE_SCALE;
fin4478 1:322c80f884d3 67
fin4478 0:4d358fbeab6e 68 m.x = (m.x - m_min.x) / (m_max.x - m_min.x) * 2 - 1.0;
fin4478 0:4d358fbeab6e 69 m.y = (m.y - m_min.y) / (m_max.y - m_min.y) * 2 - 1.0;
fin4478 0:4d358fbeab6e 70 m.z = (m.z - m_min.z) / (m_max.z - m_min.z) * 2 - 1.0;
fin4478 1:322c80f884d3 71
fin4478 0:4d358fbeab6e 72 vector_normalize(&a);
fin4478 0:4d358fbeab6e 73 vector_normalize(&m);
fin4478 0:4d358fbeab6e 74 //see appendix A in app note AN3192
fin4478 0:4d358fbeab6e 75 pitch = asin(-a.x);
fin4478 0:4d358fbeab6e 76 roll = asin(a.y/cos(pitch));
fin4478 0:4d358fbeab6e 77 float heading = 0;
fin4478 0:4d358fbeab6e 78 float xh = m.x * cos(pitch) + m.z * sin(pitch);
fin4478 0:4d358fbeab6e 79 float yh = m.x * sin(roll) * sin(pitch) + m.y * cos(roll) - m.z * sin(roll) * cos(pitch);
fin4478 1:322c80f884d3 80 //float zh = -m.x * cos(roll) * sin(pitch) + m.y * sin(roll) + m.z * cos(roll) * cos(pitch);
fin4478 0:4d358fbeab6e 81 heading = 180 * atan2(yh, xh)/PI;
fin4478 0:4d358fbeab6e 82 if (heading < 0) heading += 360;
fin4478 1:322c80f884d3 83 return heading;
fin4478 0:4d358fbeab6e 84 }
fin4478 0:4d358fbeab6e 85
fin4478 0:4d358fbeab6e 86 void LSM303::vector_cross( const Plane *a,const Plane *b, Plane *out )
fin4478 0:4d358fbeab6e 87 {
fin4478 0:4d358fbeab6e 88 out->x = a->y*b->z - a->z*b->y;
fin4478 0:4d358fbeab6e 89 out->y = a->z*b->x - a->x*b->z;
fin4478 0:4d358fbeab6e 90 out->z = a->x*b->y - a->y*b->x;
fin4478 0:4d358fbeab6e 91 }
fin4478 0:4d358fbeab6e 92
fin4478 0:4d358fbeab6e 93 float LSM303::vector_dot( const Plane *a,const Plane *b )
fin4478 0:4d358fbeab6e 94 {
fin4478 0:4d358fbeab6e 95 return a->x*b->x+a->y*b->y+a->z*b->z;
fin4478 0:4d358fbeab6e 96 }
fin4478 0:4d358fbeab6e 97
fin4478 0:4d358fbeab6e 98 void LSM303::vector_normalize( Plane *a )
fin4478 0:4d358fbeab6e 99 {
fin4478 0:4d358fbeab6e 100 float mag = sqrt(vector_dot(a,a));
fin4478 0:4d358fbeab6e 101 a->x /= mag;
fin4478 0:4d358fbeab6e 102 a->y /= mag;
fin4478 0:4d358fbeab6e 103 a->z /= mag;
fin4478 0:4d358fbeab6e 104 }
fin4478 0:4d358fbeab6e 105
fin4478 0:4d358fbeab6e 106 int LSM303::getLSM303_accel()
fin4478 0:4d358fbeab6e 107 {
fin4478 0:4d358fbeab6e 108 char data[1] = { OUT_X_L_A | (1<<7)};
fin4478 0:4d358fbeab6e 109 char out[6] = {0,0,0,0,0,0};
fin4478 0:4d358fbeab6e 110 i2c.write( LSM303_ACC, data,1);
fin4478 0:4d358fbeab6e 111 i2c.read( LSM303_ACC, out, 6);
fin4478 0:4d358fbeab6e 112
fin4478 0:4d358fbeab6e 113 a.x = short( (((short)out[1]) << 8) | out[0] );
fin4478 0:4d358fbeab6e 114 a.y = short( (((short)out[3]) << 8) | out[2] );
fin4478 0:4d358fbeab6e 115 a.z = short( (((short)out[5]) << 8) | out[4] );
fin4478 0:4d358fbeab6e 116 return 0;
fin4478 0:4d358fbeab6e 117 }
fin4478 0:4d358fbeab6e 118
fin4478 0:4d358fbeab6e 119 void LSM303::getLSM303_mag()
fin4478 0:4d358fbeab6e 120 {
fin4478 0:4d358fbeab6e 121 char data[1] = { OUT_X_H_M };
fin4478 0:4d358fbeab6e 122 char out[6];
fin4478 0:4d358fbeab6e 123
fin4478 0:4d358fbeab6e 124 i2c.write( LSM303_MAG, data, 1 );
fin4478 0:4d358fbeab6e 125 i2c.read( LSM303_MAG, out, 6 );
fin4478 0:4d358fbeab6e 126 // DLM, DLHC: register address for Z comes before Y
fin4478 0:4d358fbeab6e 127 m.x = short( out[0] << 8 | out[1] );
fin4478 0:4d358fbeab6e 128 m.y = short( out[4] << 8 | out[5] );
fin4478 0:4d358fbeab6e 129 m.z= short( out[2] << 8 | out[3] );
fin4478 0:4d358fbeab6e 130 }
fin4478 0:4d358fbeab6e 131
fin4478 0:4d358fbeab6e 132 int LSM303::LSM303_read(int address)
fin4478 0:4d358fbeab6e 133 {
fin4478 0:4d358fbeab6e 134 if (address >= 0x20) {
fin4478 0:4d358fbeab6e 135 _i2c_address = LSM303_ACC;
fin4478 0:4d358fbeab6e 136 } else {
fin4478 0:4d358fbeab6e 137 _i2c_address = LSM303_MAG;
fin4478 0:4d358fbeab6e 138 }
fin4478 0:4d358fbeab6e 139
fin4478 0:4d358fbeab6e 140 char value[1];
fin4478 0:4d358fbeab6e 141
fin4478 0:4d358fbeab6e 142 char data[1] = { address };
fin4478 0:4d358fbeab6e 143 i2c.write( _i2c_address, data, 1 );
fin4478 0:4d358fbeab6e 144 i2c.read( _i2c_address, value, 1 );
fin4478 0:4d358fbeab6e 145 return value[0];
fin4478 0:4d358fbeab6e 146 }
fin4478 0:4d358fbeab6e 147
fin4478 0:4d358fbeab6e 148 int LSM303::LSM303_write(int data, int address)
fin4478 0:4d358fbeab6e 149 {
fin4478 0:4d358fbeab6e 150 if (address >= 0x20) {
fin4478 0:4d358fbeab6e 151 _i2c_address = LSM303_ACC;
fin4478 0:4d358fbeab6e 152 } else {
fin4478 0:4d358fbeab6e 153 _i2c_address = LSM303_MAG;
fin4478 0:4d358fbeab6e 154 }
fin4478 0:4d358fbeab6e 155
fin4478 0:4d358fbeab6e 156 char out[2] = { address, data };
fin4478 0:4d358fbeab6e 157 i2c.write( _i2c_address, out, 2 );
fin4478 0:4d358fbeab6e 158 return 0;
fin4478 0:4d358fbeab6e 159 }