Compass Developer / LSM303DM

library for using LSM303DM chip

LSM303.cpp@2:1052b1b97cc2, 2013-10-13 (annotated)

Committer:
fin4478
Date:
Sun Oct 13 02:38:38 2013 +0000
Revision:
2:1052b1b97cc2
Parent:
1:322c80f884d3
Child:
3:b2cc1d06e2f5
initial publish

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