Compass Developer / LSM303DM

library for using LSM303DM chip

LSM303.cpp@4:52892e52889a, 2013-11-11 (annotated)

Committer:
fin4478
Date:
Mon Nov 11 08:46:17 2013 +0000
Revision:
4:52892e52889a
Parent:
3:b2cc1d06e2f5
Child:
5:9786e0a13a3a
tft installed and calibrated

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 4:52892e52889a 12 m_max.x = 517;
fin4478 4:52892e52889a 13 m_max.y = 469;
fin4478 4:52892e52889a 14 m_max.z = 667;
fin4478 4:52892e52889a 15 m_min.x = -688;
fin4478 4:52892e52889a 16 m_min.y = -749;
fin4478 4:52892e52889a 17 m_min.z = -472;
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 3:b2cc1d06e2f5 26 LSM303_write(0x27, CTRL_REG1_A);
fin4478 2:1052b1b97cc2 27 LSM303_write(0x00, CTRL_REG4_A);
fin4478 2:1052b1b97cc2 28 LSM303_write(MAG_SCALE_1_3, CRB_REG_M); //magnetic scale = +/-1.3Gauss
fin4478 2:1052b1b97cc2 29 LSM303_write(0x00, MR_REG_M); // 0x00 = continouous conversion mode
fin4478 2:1052b1b97cc2 30
fin4478 2:1052b1b97cc2 31 return 1; //success
fin4478 0:4d358fbeab6e 32 }
fin4478 0:4d358fbeab6e 33 int LSM303::testAcc()
fin4478 0:4d358fbeab6e 34 {
fin4478 0:4d358fbeab6e 35 if (i2c.write(LSM303_ACC, NULL, 0) ==0) return LSM303_ACC;
fin4478 0:4d358fbeab6e 36 return 255;
fin4478 0:4d358fbeab6e 37 }
fin4478 0:4d358fbeab6e 38
fin4478 0:4d358fbeab6e 39 int LSM303::testMag()
fin4478 0:4d358fbeab6e 40 {
fin4478 1:322c80f884d3 41 if (i2c.write(LSM303_MAG, NULL, 0) ==0)
fin4478 0:4d358fbeab6e 42 if (LSM303_read(LSM303_WHO_AM_I_M)==0x3C) {
fin4478 0:4d358fbeab6e 43 return LSM303_WHO_AM_I_M;
fin4478 1:322c80f884d3 44 } else {
fin4478 0:4d358fbeab6e 45 return LSM303_MAG;
fin4478 0:4d358fbeab6e 46 }
fin4478 1:322c80f884d3 47
fin4478 0:4d358fbeab6e 48 return 255;
fin4478 0:4d358fbeab6e 49 }
fin4478 0:4d358fbeab6e 50
fin4478 0:4d358fbeab6e 51 float LSM303::getTiltHeading()
fin4478 0:4d358fbeab6e 52 {
fin4478 0:4d358fbeab6e 53 //shift and scale
fin4478 0:4d358fbeab6e 54 a.x = a.x / 32768 * ACCELE_SCALE;
fin4478 0:4d358fbeab6e 55 a.y = a.y / 32768 * ACCELE_SCALE;
fin4478 1:322c80f884d3 56 a.z = a.z + 950;
fin4478 0:4d358fbeab6e 57 a.z = a.z / 32768 * ACCELE_SCALE;
fin4478 1:322c80f884d3 58
fin4478 0:4d358fbeab6e 59 m.x = (m.x - m_min.x) / (m_max.x - m_min.x) * 2 - 1.0;
fin4478 0:4d358fbeab6e 60 m.y = (m.y - m_min.y) / (m_max.y - m_min.y) * 2 - 1.0;
fin4478 0:4d358fbeab6e 61 m.z = (m.z - m_min.z) / (m_max.z - m_min.z) * 2 - 1.0;
fin4478 1:322c80f884d3 62
fin4478 0:4d358fbeab6e 63 vector_normalize(&a);
fin4478 0:4d358fbeab6e 64 vector_normalize(&m);
fin4478 0:4d358fbeab6e 65 //see appendix A in app note AN3192
fin4478 0:4d358fbeab6e 66 pitch = asin(-a.x);
fin4478 0:4d358fbeab6e 67 roll = asin(a.y/cos(pitch));
fin4478 0:4d358fbeab6e 68 float heading = 0;
fin4478 0:4d358fbeab6e 69 float xh = m.x * cos(pitch) + m.z * sin(pitch);
fin4478 0:4d358fbeab6e 70 float yh = m.x * sin(roll) * sin(pitch) + m.y * cos(roll) - m.z * sin(roll) * cos(pitch);
fin4478 1:322c80f884d3 71 //float zh = -m.x * cos(roll) * sin(pitch) + m.y * sin(roll) + m.z * cos(roll) * cos(pitch);
fin4478 0:4d358fbeab6e 72 heading = 180 * atan2(yh, xh)/PI;
fin4478 0:4d358fbeab6e 73 if (heading < 0) heading += 360;
fin4478 1:322c80f884d3 74 return heading;
fin4478 0:4d358fbeab6e 75 }
fin4478 0:4d358fbeab6e 76
fin4478 0:4d358fbeab6e 77 void LSM303::vector_cross( const Plane *a,const Plane *b, Plane *out )
fin4478 0:4d358fbeab6e 78 {
fin4478 0:4d358fbeab6e 79 out->x = a->y*b->z - a->z*b->y;
fin4478 0:4d358fbeab6e 80 out->y = a->z*b->x - a->x*b->z;
fin4478 0:4d358fbeab6e 81 out->z = a->x*b->y - a->y*b->x;
fin4478 0:4d358fbeab6e 82 }
fin4478 0:4d358fbeab6e 83
fin4478 0:4d358fbeab6e 84 float LSM303::vector_dot( const Plane *a,const Plane *b )
fin4478 0:4d358fbeab6e 85 {
fin4478 0:4d358fbeab6e 86 return a->x*b->x+a->y*b->y+a->z*b->z;
fin4478 0:4d358fbeab6e 87 }
fin4478 0:4d358fbeab6e 88
fin4478 0:4d358fbeab6e 89 void LSM303::vector_normalize( Plane *a )
fin4478 0:4d358fbeab6e 90 {
fin4478 0:4d358fbeab6e 91 float mag = sqrt(vector_dot(a,a));
fin4478 0:4d358fbeab6e 92 a->x /= mag;
fin4478 0:4d358fbeab6e 93 a->y /= mag;
fin4478 0:4d358fbeab6e 94 a->z /= mag;
fin4478 0:4d358fbeab6e 95 }
fin4478 0:4d358fbeab6e 96
fin4478 3:b2cc1d06e2f5 97 void LSM303::getLSM303_accel()
fin4478 3:b2cc1d06e2f5 98 {
fin4478 0:4d358fbeab6e 99 char data[1] = { OUT_X_L_A | (1<<7)};
fin4478 0:4d358fbeab6e 100 char out[6] = {0,0,0,0,0,0};
fin4478 0:4d358fbeab6e 101 i2c.write( LSM303_ACC, data,1);
fin4478 0:4d358fbeab6e 102 i2c.read( LSM303_ACC, out, 6);
fin4478 0:4d358fbeab6e 103
fin4478 0:4d358fbeab6e 104 a.x = short( (((short)out[1]) << 8) | out[0] );
fin4478 0:4d358fbeab6e 105 a.y = short( (((short)out[3]) << 8) | out[2] );
fin4478 3:b2cc1d06e2f5 106 a.z = short( (((short)out[5]) << 8) | out[4] );
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 }