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?
User | Revision | Line number | New 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 | } |