Compass Developer / LSM303DM

library for using LSM303DM chip

LSM303.cpp@6:22556393747b, 2014-09-08 (annotated)

Committer:
fin4478
Date:
Mon Sep 08 01:46:11 2014 +0000
Revision:
6:22556393747b
Parent:
5:9786e0a13a3a 
soft iron correction scale 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 6:22556393747b 8 void 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 6:22556393747b 25 #else
fin4478 6:22556393747b 26 m_min.x = -690;
fin4478 6:22556393747b 27 m_min.y = -702;
fin4478 6:22556393747b 28 m_min.z = -433;
fin4478 6:22556393747b 29 m_max.x = 480;
fin4478 6:22556393747b 30 m_max.y = 414;
fin4478 6:22556393747b 31 m_max.z = 589;
fin4478 5:9786e0a13a3a 32
fin4478 6:22556393747b 33 a_min.x = -542;
fin4478 6:22556393747b 34 a_min.y = -644;
fin4478 6:22556393747b 35 a_min.z = -632;
fin4478 6:22556393747b 36 a_max.x = 496;
fin4478 6:22556393747b 37 a_max.y = 472;
fin4478 6:22556393747b 38 a_max.z = 566;
fin4478 0:4d358fbeab6e 39 #endif
fin4478 6:22556393747b 40 getScale(&scale);
fin4478 3:b2cc1d06e2f5 41 LSM303_write(0x27, CTRL_REG1_A);
fin4478 2:1052b1b97cc2 42 LSM303_write(0x00, CTRL_REG4_A);
fin4478 6:22556393747b 43 LSM303_write(MAG_SCALE_1_3 , CRB_REG_M); //magnetic scale = +/-1.3Gauss
fin4478 2:1052b1b97cc2 44 LSM303_write(0x00, MR_REG_M); // 0x00 = continouous conversion mode
fin4478 6:22556393747b 45 }
fin4478 2:1052b1b97cc2 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 6:22556393747b 65
fin4478 6:22556393747b 66 void LSM303::getScale(Plane *scale) {
fin4478 6:22556393747b 67 Plane vmax;
fin4478 6:22556393747b 68 Plane vmin;
fin4478 6:22556393747b 69 Plane avgs;
fin4478 6:22556393747b 70 //First the hard iron errors are removed from the maximums and minimum magnetometer vectors.
fin4478 6:22556393747b 71 //These minimum and maximum vectors are the same as the ones being used to correct for hard iron errors.
fin4478 6:22556393747b 72 vmax.x= m_max.x - ((m_min.x + m_max.x)/2.0);
fin4478 6:22556393747b 73 vmax.y= m_max.y - ((m_min.y + m_max.y)/2.0);
fin4478 6:22556393747b 74 vmax.z =m_max.z - ((m_min.z + m_max.z)/2.0);
fin4478 6:22556393747b 75
fin4478 6:22556393747b 76 vmin.x = m_min.x - ((m_min.x + m_max.x)/2.0);
fin4478 6:22556393747b 77 vmin.y = m_min.y - ((m_min.y + m_max.y)/2.0);
fin4478 6:22556393747b 78 vmin.z = m_min.z - ((m_min.z + m_max.z)/2.0);
fin4478 6:22556393747b 79
fin4478 6:22556393747b 80 //The average distance from the centre is now calculated. We want to know how far from the centre, so the negative values are inverted.
fin4478 6:22556393747b 81 //avgs = vmax + (vmin*-1); //multiply by -1 to make negative values positive
fin4478 6:22556393747b 82 //avgs = avgs / 2.0;
fin4478 6:22556393747b 83 avgs.x = (vmax.x + vmin.x*-1)/2.0;
fin4478 6:22556393747b 84 avgs.y = (vmax.y + vmin.y*-1)/2.0;
fin4478 6:22556393747b 85 avgs.z = (vmax.z + vmin.z*-1)/2.0;
fin4478 6:22556393747b 86 //The components are now averaged out
fin4478 6:22556393747b 87 float avg_rad = avgs.x + avgs.y + avgs.z;
fin4478 6:22556393747b 88 avg_rad /= 3.0;
fin4478 6:22556393747b 89 //Finally calculate the scale factor by dividing average radius by average value for that axis.
fin4478 6:22556393747b 90 scale->x = (avg_rad/avgs.x);
fin4478 6:22556393747b 91 scale->y = (avg_rad/avgs.y);
fin4478 6:22556393747b 92 scale->z = (avg_rad/avgs.z);
fin4478 6:22556393747b 93 }
fin4478 6:22556393747b 94
fin4478 0:4d358fbeab6e 95 float LSM303::getTiltHeading()
fin4478 0:4d358fbeab6e 96 {
fin4478 6:22556393747b 97 getLSM303_accel();
fin4478 6:22556393747b 98 getLSM303_mag(); // get the accel and magnetometer values, store them in a and m
fin4478 6:22556393747b 99
fin4478 5:9786e0a13a3a 100 a.x -= ((int32_t)a_min.x + a_max.x) / 2;
fin4478 5:9786e0a13a3a 101 a.y -= ((int32_t)a_min.y + a_max.y) / 2;
fin4478 5:9786e0a13a3a 102 a.z -= ((int32_t)a_min.z + a_max.z) / 2;
fin4478 6:22556393747b 103
fin4478 5:9786e0a13a3a 104 // subtract offset (average of min and max) from magnetometer readings
fin4478 5:9786e0a13a3a 105 m.x -= ((int32_t)m_min.x + m_max.x) / 2;
fin4478 5:9786e0a13a3a 106 m.y -= ((int32_t)m_min.y + m_max.y) / 2;
fin4478 5:9786e0a13a3a 107 m.z -= ((int32_t)m_min.z + m_max.z) / 2;
fin4478 5:9786e0a13a3a 108
fin4478 6:22556393747b 109 m.x *= scale.x;
fin4478 6:22556393747b 110 m.y *= scale.y;
fin4478 6:22556393747b 111 m.z *= scale.z;
fin4478 6:22556393747b 112
fin4478 0:4d358fbeab6e 113 vector_normalize(&a);
fin4478 0:4d358fbeab6e 114 vector_normalize(&m);
fin4478 0:4d358fbeab6e 115 //see appendix A in app note AN3192
fin4478 0:4d358fbeab6e 116 pitch = asin(-a.x);
fin4478 0:4d358fbeab6e 117 roll = asin(a.y/cos(pitch));
fin4478 0:4d358fbeab6e 118 float heading = 0;
fin4478 0:4d358fbeab6e 119 float xh = m.x * cos(pitch) + m.z * sin(pitch);
fin4478 0:4d358fbeab6e 120 float yh = m.x * sin(roll) * sin(pitch) + m.y * cos(roll) - m.z * sin(roll) * cos(pitch);
fin4478 5:9786e0a13a3a 121
fin4478 0:4d358fbeab6e 122 heading = 180 * atan2(yh, xh)/PI;
fin4478 0:4d358fbeab6e 123 if (heading < 0) heading += 360;
fin4478 5:9786e0a13a3a 124
fin4478 1:322c80f884d3 125 return heading;
fin4478 0:4d358fbeab6e 126 }
fin4478 0:4d358fbeab6e 127
fin4478 0:4d358fbeab6e 128 void LSM303::vector_cross( const Plane *a,const Plane *b, Plane *out )
fin4478 0:4d358fbeab6e 129 {
fin4478 0:4d358fbeab6e 130 out->x = a->y*b->z - a->z*b->y;
fin4478 0:4d358fbeab6e 131 out->y = a->z*b->x - a->x*b->z;
fin4478 0:4d358fbeab6e 132 out->z = a->x*b->y - a->y*b->x;
fin4478 0:4d358fbeab6e 133 }
fin4478 0:4d358fbeab6e 134
fin4478 0:4d358fbeab6e 135 float LSM303::vector_dot( const Plane *a,const Plane *b )
fin4478 0:4d358fbeab6e 136 {
fin4478 0:4d358fbeab6e 137 return a->x*b->x+a->y*b->y+a->z*b->z;
fin4478 0:4d358fbeab6e 138 }
fin4478 0:4d358fbeab6e 139
fin4478 0:4d358fbeab6e 140 void LSM303::vector_normalize( Plane *a )
fin4478 0:4d358fbeab6e 141 {
fin4478 0:4d358fbeab6e 142 float mag = sqrt(vector_dot(a,a));
fin4478 0:4d358fbeab6e 143 a->x /= mag;
fin4478 0:4d358fbeab6e 144 a->y /= mag;
fin4478 0:4d358fbeab6e 145 a->z /= mag;
fin4478 0:4d358fbeab6e 146 }
fin4478 0:4d358fbeab6e 147
fin4478 3:b2cc1d06e2f5 148 void LSM303::getLSM303_accel()
fin4478 3:b2cc1d06e2f5 149 {
fin4478 0:4d358fbeab6e 150 char data[1] = { OUT_X_L_A | (1<<7)};
fin4478 0:4d358fbeab6e 151 char out[6] = {0,0,0,0,0,0};
fin4478 0:4d358fbeab6e 152 i2c.write( LSM303_ACC, data,1);
fin4478 0:4d358fbeab6e 153 i2c.read( LSM303_ACC, out, 6);
fin4478 0:4d358fbeab6e 154
fin4478 0:4d358fbeab6e 155 a.x = short( (((short)out[1]) << 8) | out[0] );
fin4478 0:4d358fbeab6e 156 a.y = short( (((short)out[3]) << 8) | out[2] );
fin4478 3:b2cc1d06e2f5 157 a.z = short( (((short)out[5]) << 8) | out[4] );
fin4478 0:4d358fbeab6e 158 }
fin4478 0:4d358fbeab6e 159
fin4478 0:4d358fbeab6e 160 void LSM303::getLSM303_mag()
fin4478 0:4d358fbeab6e 161 {
fin4478 0:4d358fbeab6e 162 char data[1] = { OUT_X_H_M };
fin4478 0:4d358fbeab6e 163 char out[6];
fin4478 0:4d358fbeab6e 164
fin4478 0:4d358fbeab6e 165 i2c.write( LSM303_MAG, data, 1 );
fin4478 0:4d358fbeab6e 166 i2c.read( LSM303_MAG, out, 6 );
fin4478 0:4d358fbeab6e 167 // DLM, DLHC: register address for Z comes before Y
fin4478 0:4d358fbeab6e 168 m.x = short( out[0] << 8 | out[1] );
fin4478 0:4d358fbeab6e 169 m.y = short( out[4] << 8 | out[5] );
fin4478 0:4d358fbeab6e 170 m.z= short( out[2] << 8 | out[3] );
fin4478 0:4d358fbeab6e 171 }
fin4478 0:4d358fbeab6e 172
fin4478 0:4d358fbeab6e 173 int LSM303::LSM303_read(int address)
fin4478 0:4d358fbeab6e 174 {
fin4478 0:4d358fbeab6e 175 if (address >= 0x20) {
fin4478 0:4d358fbeab6e 176 _i2c_address = LSM303_ACC;
fin4478 0:4d358fbeab6e 177 } else {
fin4478 0:4d358fbeab6e 178 _i2c_address = LSM303_MAG;
fin4478 0:4d358fbeab6e 179 }
fin4478 0:4d358fbeab6e 180
fin4478 0:4d358fbeab6e 181 char value[1];
fin4478 0:4d358fbeab6e 182
fin4478 0:4d358fbeab6e 183 char data[1] = { address };
fin4478 0:4d358fbeab6e 184 i2c.write( _i2c_address, data, 1 );
fin4478 0:4d358fbeab6e 185 i2c.read( _i2c_address, value, 1 );
fin4478 0:4d358fbeab6e 186 return value[0];
fin4478 0:4d358fbeab6e 187 }
fin4478 0:4d358fbeab6e 188
fin4478 0:4d358fbeab6e 189 int LSM303::LSM303_write(int data, int address)
fin4478 0:4d358fbeab6e 190 {
fin4478 0:4d358fbeab6e 191 if (address >= 0x20) {
fin4478 0:4d358fbeab6e 192 _i2c_address = LSM303_ACC;
fin4478 0:4d358fbeab6e 193 } else {
fin4478 0:4d358fbeab6e 194 _i2c_address = LSM303_MAG;
fin4478 0:4d358fbeab6e 195 }
fin4478 0:4d358fbeab6e 196
fin4478 0:4d358fbeab6e 197 char out[2] = { address, data };
fin4478 0:4d358fbeab6e 198 i2c.write( _i2c_address, out, 2 );
fin4478 0:4d358fbeab6e 199 return 0;
fin4478 0:4d358fbeab6e 200 }