Compass Developer / LSM303DM

library for using LSM303DM chip

LSM303.cpp@5:9786e0a13a3a, 2014-08-12 (annotated)

Committer:
fin4478
Date:
Tue Aug 12 11:43:45 2014 +0000
Revision:
5:9786e0a13a3a
Parent:
4:52892e52889a
Child:
6:22556393747b
accelometer calibration added

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