AHRS library for the Polulu minIMU-9 Ability to interface with the Polulu Python minIMU-9 monitor
LSM303.cpp@1:3272ece36ce1, 2012-04-23 (annotated)
- Committer:
- krmreynolds
- Date:
- Mon Apr 23 14:31:08 2012 +0000
- Revision:
- 1:3272ece36ce1
- Parent:
- 0:dc35364e2291
Ported L3G4200D and LSM303 from the Polulu website, dropped the horrible code that someone else built
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
krmreynolds | 0:dc35364e2291 | 1 | #include <LSM303.h> |
krmreynolds | 1:3272ece36ce1 | 2 | #include "LSM303.h" |
krmreynolds | 1:3272ece36ce1 | 3 | #include <math.h> |
krmreynolds | 1:3272ece36ce1 | 4 | |
krmreynolds | 1:3272ece36ce1 | 5 | LSM303::LSM303( PinName sda = p9, PinName scl = p10 ) : i2c( sda, scl ) { |
krmreynolds | 1:3272ece36ce1 | 6 | // These are just some values for a particular unit; it is recommended that |
krmreynolds | 1:3272ece36ce1 | 7 | // a calibration be done for your particular unit. |
krmreynolds | 1:3272ece36ce1 | 8 | m_max.x = +540; |
krmreynolds | 1:3272ece36ce1 | 9 | m_max.y = +500; |
krmreynolds | 1:3272ece36ce1 | 10 | m_max.z = 180; |
krmreynolds | 1:3272ece36ce1 | 11 | m_min.x = -520; |
krmreynolds | 1:3272ece36ce1 | 12 | m_min.y = -570; |
krmreynolds | 1:3272ece36ce1 | 13 | m_min.z = -770; |
krmreynolds | 1:3272ece36ce1 | 14 | |
krmreynolds | 1:3272ece36ce1 | 15 | _device = LSM303DLM_DEVICE; |
krmreynolds | 1:3272ece36ce1 | 16 | acc_address = ACC_ADDRESS_SA0_A_LOW; |
krmreynolds | 1:3272ece36ce1 | 17 | } |
krmreynolds | 1:3272ece36ce1 | 18 | |
krmreynolds | 1:3272ece36ce1 | 19 | // Public Methods ////////////////////////////////////////////////////////////// |
krmreynolds | 1:3272ece36ce1 | 20 | |
krmreynolds | 1:3272ece36ce1 | 21 | void LSM303::init(byte device, byte sa0_a) { |
krmreynolds | 1:3272ece36ce1 | 22 | Serial pc(USBTX, USBRX); |
krmreynolds | 1:3272ece36ce1 | 23 | _device = device; |
krmreynolds | 1:3272ece36ce1 | 24 | switch (_device) { |
krmreynolds | 1:3272ece36ce1 | 25 | case LSM303DLH_DEVICE: |
krmreynolds | 1:3272ece36ce1 | 26 | case LSM303DLM_DEVICE: |
krmreynolds | 1:3272ece36ce1 | 27 | if (sa0_a == LSM303_SA0_A_LOW) |
krmreynolds | 1:3272ece36ce1 | 28 | acc_address = ACC_ADDRESS_SA0_A_LOW; |
krmreynolds | 1:3272ece36ce1 | 29 | else if (sa0_a == LSM303_SA0_A_HIGH) |
krmreynolds | 1:3272ece36ce1 | 30 | acc_address = ACC_ADDRESS_SA0_A_HIGH; |
krmreynolds | 1:3272ece36ce1 | 31 | else |
krmreynolds | 1:3272ece36ce1 | 32 | acc_address = (detectSA0_A() == LSM303_SA0_A_HIGH) ? ACC_ADDRESS_SA0_A_HIGH : ACC_ADDRESS_SA0_A_LOW; |
krmreynolds | 1:3272ece36ce1 | 33 | break; |
krmreynolds | 1:3272ece36ce1 | 34 | |
krmreynolds | 1:3272ece36ce1 | 35 | case LSM303DLHC_DEVICE: |
krmreynolds | 1:3272ece36ce1 | 36 | acc_address = ACC_ADDRESS_SA0_A_HIGH; |
krmreynolds | 1:3272ece36ce1 | 37 | break; |
krmreynolds | 0:dc35364e2291 | 38 | |
krmreynolds | 1:3272ece36ce1 | 39 | default: |
krmreynolds | 1:3272ece36ce1 | 40 | // try to auto-detect device |
krmreynolds | 1:3272ece36ce1 | 41 | if (detectSA0_A() == LSM303_SA0_A_HIGH) { |
krmreynolds | 1:3272ece36ce1 | 42 | // if device responds on 0011001b (SA0_A is high), assume DLHC |
krmreynolds | 1:3272ece36ce1 | 43 | acc_address = ACC_ADDRESS_SA0_A_HIGH; |
krmreynolds | 1:3272ece36ce1 | 44 | _device = LSM303DLHC_DEVICE; |
krmreynolds | 1:3272ece36ce1 | 45 | } else { |
krmreynolds | 1:3272ece36ce1 | 46 | // otherwise, assume DLH or DLM (pulled low by default on Pololu boards); query magnetometer WHO_AM_I to differentiate these two |
krmreynolds | 1:3272ece36ce1 | 47 | acc_address = ACC_ADDRESS_SA0_A_LOW; |
krmreynolds | 1:3272ece36ce1 | 48 | _device = (readMagReg(LSM303_WHO_AM_I_M) == 0x3C) ? LSM303DLM_DEVICE : LSM303DLH_DEVICE; |
krmreynolds | 1:3272ece36ce1 | 49 | } |
krmreynolds | 1:3272ece36ce1 | 50 | } |
krmreynolds | 1:3272ece36ce1 | 51 | } |
krmreynolds | 1:3272ece36ce1 | 52 | |
krmreynolds | 1:3272ece36ce1 | 53 | // Turns on the LSM303's accelerometer and magnetometers and places them in normal |
krmreynolds | 1:3272ece36ce1 | 54 | // mode. |
krmreynolds | 1:3272ece36ce1 | 55 | void LSM303::enableDefault(void) { |
krmreynolds | 1:3272ece36ce1 | 56 | // Enable Accelerometer |
krmreynolds | 1:3272ece36ce1 | 57 | // 0x27 = 0b00100111 |
krmreynolds | 1:3272ece36ce1 | 58 | // Normal power mode, all axes enabled |
krmreynolds | 1:3272ece36ce1 | 59 | writeAccReg(LSM303_CTRL_REG1_A, 0x27); |
krmreynolds | 0:dc35364e2291 | 60 | |
krmreynolds | 1:3272ece36ce1 | 61 | // Enable Magnetometer |
krmreynolds | 1:3272ece36ce1 | 62 | // 0x00 = 0b00000000 |
krmreynolds | 1:3272ece36ce1 | 63 | // Continuous conversion mode |
krmreynolds | 1:3272ece36ce1 | 64 | writeMagReg(LSM303_MR_REG_M, 0x00); |
krmreynolds | 1:3272ece36ce1 | 65 | } |
krmreynolds | 1:3272ece36ce1 | 66 | |
krmreynolds | 1:3272ece36ce1 | 67 | // Writes an accelerometer register |
krmreynolds | 1:3272ece36ce1 | 68 | void LSM303::writeAccReg(byte reg, byte value) { |
krmreynolds | 1:3272ece36ce1 | 69 | char data[2] = { reg, value }; |
krmreynolds | 1:3272ece36ce1 | 70 | i2c.write( acc_address, data, 2 ); |
krmreynolds | 0:dc35364e2291 | 71 | } |
krmreynolds | 1:3272ece36ce1 | 72 | |
krmreynolds | 1:3272ece36ce1 | 73 | // Reads an accelerometer register |
krmreynolds | 1:3272ece36ce1 | 74 | int LSM303::readAccReg(byte reg) { |
krmreynolds | 1:3272ece36ce1 | 75 | char value[1]; |
krmreynolds | 1:3272ece36ce1 | 76 | char data[1] = { reg }; |
krmreynolds | 1:3272ece36ce1 | 77 | i2c.write( acc_address, data, 1 ); |
krmreynolds | 1:3272ece36ce1 | 78 | i2c.read( acc_address, value, 1 ); |
krmreynolds | 1:3272ece36ce1 | 79 | return value[0]; |
krmreynolds | 1:3272ece36ce1 | 80 | } |
krmreynolds | 1:3272ece36ce1 | 81 | |
krmreynolds | 1:3272ece36ce1 | 82 | // Writes a magnetometer register |
krmreynolds | 1:3272ece36ce1 | 83 | void LSM303::writeMagReg(byte reg, byte value) { |
krmreynolds | 1:3272ece36ce1 | 84 | char data[2] = { reg, value }; |
krmreynolds | 1:3272ece36ce1 | 85 | i2c.write( MAG_ADDRESS, data, 2 ); |
krmreynolds | 0:dc35364e2291 | 86 | } |
krmreynolds | 0:dc35364e2291 | 87 | |
krmreynolds | 1:3272ece36ce1 | 88 | // Reads a magnetometer register |
krmreynolds | 1:3272ece36ce1 | 89 | int LSM303::readMagReg(int reg) { |
krmreynolds | 1:3272ece36ce1 | 90 | char value[1]; |
krmreynolds | 1:3272ece36ce1 | 91 | char data[1]; |
krmreynolds | 1:3272ece36ce1 | 92 | // if dummy register address (magnetometer Y/Z), use device type to determine actual address |
krmreynolds | 1:3272ece36ce1 | 93 | if (reg < 0) { |
krmreynolds | 1:3272ece36ce1 | 94 | switch (reg) { |
krmreynolds | 1:3272ece36ce1 | 95 | case LSM303_OUT_Y_H_M: |
krmreynolds | 1:3272ece36ce1 | 96 | reg = (_device == LSM303DLH_DEVICE) ? LSM303DLH_OUT_Y_H_M : LSM303DLM_OUT_Y_H_M; |
krmreynolds | 1:3272ece36ce1 | 97 | break; |
krmreynolds | 1:3272ece36ce1 | 98 | case LSM303_OUT_Y_L_M: |
krmreynolds | 1:3272ece36ce1 | 99 | reg = (_device == LSM303DLH_DEVICE) ? LSM303DLH_OUT_Y_L_M : LSM303DLM_OUT_Y_L_M; |
krmreynolds | 1:3272ece36ce1 | 100 | break; |
krmreynolds | 1:3272ece36ce1 | 101 | case LSM303_OUT_Z_H_M: |
krmreynolds | 1:3272ece36ce1 | 102 | reg = (_device == LSM303DLH_DEVICE) ? LSM303DLH_OUT_Z_H_M : LSM303DLM_OUT_Z_H_M; |
krmreynolds | 1:3272ece36ce1 | 103 | break; |
krmreynolds | 1:3272ece36ce1 | 104 | case LSM303_OUT_Z_L_M: |
krmreynolds | 1:3272ece36ce1 | 105 | reg = (_device == LSM303DLH_DEVICE) ? LSM303DLH_OUT_Z_L_M : LSM303DLM_OUT_Z_L_M; |
krmreynolds | 1:3272ece36ce1 | 106 | break; |
krmreynolds | 1:3272ece36ce1 | 107 | } |
krmreynolds | 1:3272ece36ce1 | 108 | } |
krmreynolds | 1:3272ece36ce1 | 109 | data[0] = reg; |
krmreynolds | 1:3272ece36ce1 | 110 | i2c.write( MAG_ADDRESS, data, 1 ); |
krmreynolds | 1:3272ece36ce1 | 111 | i2c.read( MAG_ADDRESS, value, 1 ); |
krmreynolds | 1:3272ece36ce1 | 112 | return value[0]; |
krmreynolds | 1:3272ece36ce1 | 113 | |
krmreynolds | 1:3272ece36ce1 | 114 | } |
krmreynolds | 1:3272ece36ce1 | 115 | |
krmreynolds | 1:3272ece36ce1 | 116 | // Reads the 3 accelerometer channels and stores them in vector a |
krmreynolds | 1:3272ece36ce1 | 117 | void LSM303::readAcc(void) { |
krmreynolds | 1:3272ece36ce1 | 118 | char data[1] = { LSM303_OUT_X_L_A | (1<<7) }; |
krmreynolds | 1:3272ece36ce1 | 119 | char out[6]; |
krmreynolds | 1:3272ece36ce1 | 120 | i2c.write( acc_address, data, 1 ); |
krmreynolds | 1:3272ece36ce1 | 121 | |
krmreynolds | 1:3272ece36ce1 | 122 | i2c.read( acc_address, out, 6 ); |
krmreynolds | 1:3272ece36ce1 | 123 | |
krmreynolds | 1:3272ece36ce1 | 124 | a.x = short( ( out[1] << 8 | out[0] ) >> 4 ); |
krmreynolds | 1:3272ece36ce1 | 125 | a.y = short( ( out[3] << 8 | out[2] ) >> 4 ); |
krmreynolds | 1:3272ece36ce1 | 126 | a.z = short( ( out[5] << 8 | out[4] ) >> 4 ); |
krmreynolds | 1:3272ece36ce1 | 127 | } |
krmreynolds | 1:3272ece36ce1 | 128 | |
krmreynolds | 1:3272ece36ce1 | 129 | // Reads the 3 magnetometer channels and stores them in vector m |
krmreynolds | 1:3272ece36ce1 | 130 | void LSM303::readMag(void) { |
krmreynolds | 1:3272ece36ce1 | 131 | Serial pc(USBTX, USBRX); |
krmreynolds | 1:3272ece36ce1 | 132 | char data[1] = { LSM303_OUT_X_H_M }; |
krmreynolds | 1:3272ece36ce1 | 133 | char out[6]; |
krmreynolds | 1:3272ece36ce1 | 134 | |
krmreynolds | 1:3272ece36ce1 | 135 | i2c.write( MAG_ADDRESS, data, 1 ); |
krmreynolds | 1:3272ece36ce1 | 136 | i2c.read( MAG_ADDRESS, out, 6 ); |
krmreynolds | 1:3272ece36ce1 | 137 | |
krmreynolds | 1:3272ece36ce1 | 138 | if (_device == LSM303DLH_DEVICE) { |
krmreynolds | 1:3272ece36ce1 | 139 | // DLH: register address for Y comes before Z |
krmreynolds | 1:3272ece36ce1 | 140 | m.x = short( out[0] << 8 | out[1] ); |
krmreynolds | 1:3272ece36ce1 | 141 | m.y = short( out[2] << 8 | out[3] ); |
krmreynolds | 1:3272ece36ce1 | 142 | m.z = short( out[4] << 8 | out[5] ); |
krmreynolds | 1:3272ece36ce1 | 143 | } else { |
krmreynolds | 1:3272ece36ce1 | 144 | // DLM, DLHC: register address for Z comes before Y |
krmreynolds | 1:3272ece36ce1 | 145 | m.x = short( out[0] << 8 | out[1] ); |
krmreynolds | 1:3272ece36ce1 | 146 | m.y = short( out[4] << 8 | out[5] ); |
krmreynolds | 1:3272ece36ce1 | 147 | m.z = short( out[2] << 8 | out[3] ); |
krmreynolds | 1:3272ece36ce1 | 148 | } |
krmreynolds | 0:dc35364e2291 | 149 | } |
krmreynolds | 0:dc35364e2291 | 150 | |
krmreynolds | 1:3272ece36ce1 | 151 | // Reads all 6 channels of the LSM303 and stores them in the object variables |
krmreynolds | 1:3272ece36ce1 | 152 | void LSM303::read(void) { |
krmreynolds | 1:3272ece36ce1 | 153 | readAcc(); |
krmreynolds | 1:3272ece36ce1 | 154 | readMag(); |
krmreynolds | 1:3272ece36ce1 | 155 | } |
krmreynolds | 1:3272ece36ce1 | 156 | |
krmreynolds | 1:3272ece36ce1 | 157 | // Returns the number of degrees from the -Y axis that it |
krmreynolds | 1:3272ece36ce1 | 158 | // is pointing. |
krmreynolds | 1:3272ece36ce1 | 159 | int LSM303::heading(void) { |
krmreynolds | 1:3272ece36ce1 | 160 | return heading((Plane) { |
krmreynolds | 1:3272ece36ce1 | 161 | 0,-1,0 |
krmreynolds | 1:3272ece36ce1 | 162 | }); |
krmreynolds | 0:dc35364e2291 | 163 | } |
krmreynolds | 0:dc35364e2291 | 164 | |
krmreynolds | 1:3272ece36ce1 | 165 | // Returns the number of degrees from the From vector projected into |
krmreynolds | 1:3272ece36ce1 | 166 | // the horizontal plane is away from north. |
krmreynolds | 1:3272ece36ce1 | 167 | // |
krmreynolds | 1:3272ece36ce1 | 168 | // Description of heading algorithm: |
krmreynolds | 1:3272ece36ce1 | 169 | // Shift and scale the magnetic reading based on calibration data to |
krmreynolds | 1:3272ece36ce1 | 170 | // to find the North vector. Use the acceleration readings to |
krmreynolds | 1:3272ece36ce1 | 171 | // determine the Down vector. The cross product of North and Down |
krmreynolds | 1:3272ece36ce1 | 172 | // vectors is East. The vectors East and North form a basis for the |
krmreynolds | 1:3272ece36ce1 | 173 | // horizontal plane. The From vector is projected into the horizontal |
krmreynolds | 1:3272ece36ce1 | 174 | // plane and the angle between the projected vector and north is |
krmreynolds | 1:3272ece36ce1 | 175 | // returned. |
krmreynolds | 1:3272ece36ce1 | 176 | int LSM303::heading(Plane from) { |
krmreynolds | 1:3272ece36ce1 | 177 | // shift and scale |
krmreynolds | 1:3272ece36ce1 | 178 | m.x = (m.x - m_min.x) / (m_max.x - m_min.x) * 2 - 1.0; |
krmreynolds | 1:3272ece36ce1 | 179 | m.y = (m.y - m_min.y) / (m_max.y - m_min.y) * 2 - 1.0; |
krmreynolds | 1:3272ece36ce1 | 180 | m.z = (m.z - m_min.z) / (m_max.z - m_min.z) * 2 - 1.0; |
krmreynolds | 1:3272ece36ce1 | 181 | |
krmreynolds | 1:3272ece36ce1 | 182 | Plane temp_a = a; |
krmreynolds | 1:3272ece36ce1 | 183 | // normalize |
krmreynolds | 1:3272ece36ce1 | 184 | vector_normalize(&temp_a); |
krmreynolds | 1:3272ece36ce1 | 185 | //vector_normalize(&m); |
krmreynolds | 1:3272ece36ce1 | 186 | |
krmreynolds | 1:3272ece36ce1 | 187 | // compute E and N |
krmreynolds | 1:3272ece36ce1 | 188 | Plane E; |
krmreynolds | 1:3272ece36ce1 | 189 | Plane N; |
krmreynolds | 1:3272ece36ce1 | 190 | vector_cross(&m, &temp_a, &E); |
krmreynolds | 1:3272ece36ce1 | 191 | vector_normalize(&E); |
krmreynolds | 1:3272ece36ce1 | 192 | vector_cross(&temp_a, &E, &N); |
krmreynolds | 1:3272ece36ce1 | 193 | |
krmreynolds | 1:3272ece36ce1 | 194 | // compute heading |
krmreynolds | 1:3272ece36ce1 | 195 | int heading = (int)(atan2(vector_dot(&E, &from), vector_dot(&N, &from)) * 180 / M_PI); |
krmreynolds | 1:3272ece36ce1 | 196 | if (heading < 0) heading += 360; |
krmreynolds | 1:3272ece36ce1 | 197 | return heading; |
krmreynolds | 1:3272ece36ce1 | 198 | } |
krmreynolds | 1:3272ece36ce1 | 199 | |
krmreynolds | 1:3272ece36ce1 | 200 | void LSM303::vector_cross( const Plane *a,const Plane *b, Plane *out ) { |
krmreynolds | 1:3272ece36ce1 | 201 | out->x = a->y*b->z - a->z*b->y; |
krmreynolds | 1:3272ece36ce1 | 202 | out->y = a->z*b->x - a->x*b->z; |
krmreynolds | 1:3272ece36ce1 | 203 | out->z = a->x*b->y - a->y*b->x; |
krmreynolds | 1:3272ece36ce1 | 204 | } |
krmreynolds | 1:3272ece36ce1 | 205 | |
krmreynolds | 1:3272ece36ce1 | 206 | float LSM303::vector_dot( const Plane *a,const Plane *b ) { |
krmreynolds | 1:3272ece36ce1 | 207 | return a->x*b->x+a->y*b->y+a->z*b->z; |
krmreynolds | 1:3272ece36ce1 | 208 | } |
krmreynolds | 1:3272ece36ce1 | 209 | |
krmreynolds | 1:3272ece36ce1 | 210 | void LSM303::vector_normalize( Plane *a ) { |
krmreynolds | 1:3272ece36ce1 | 211 | float mag = sqrt(vector_dot(a,a)); |
krmreynolds | 1:3272ece36ce1 | 212 | a->x /= mag; |
krmreynolds | 1:3272ece36ce1 | 213 | a->y /= mag; |
krmreynolds | 1:3272ece36ce1 | 214 | a->z /= mag; |
krmreynolds | 0:dc35364e2291 | 215 | } |
krmreynolds | 0:dc35364e2291 | 216 | |
krmreynolds | 0:dc35364e2291 | 217 | |
krmreynolds | 1:3272ece36ce1 | 218 | byte LSM303::detectSA0_A(void) { |
krmreynolds | 1:3272ece36ce1 | 219 | char out[1]; |
krmreynolds | 1:3272ece36ce1 | 220 | char data[1] = { LSM303_CTRL_REG1_A }; |
krmreynolds | 1:3272ece36ce1 | 221 | i2c.write( ACC_ADDRESS_SA0_A_LOW, data, 1 ); |
krmreynolds | 1:3272ece36ce1 | 222 | |
krmreynolds | 1:3272ece36ce1 | 223 | if ( 0 == i2c.read( ACC_ADDRESS_SA0_A_LOW, out, 1 ) ) { |
krmreynolds | 1:3272ece36ce1 | 224 | return LSM303_SA0_A_LOW; |
krmreynolds | 1:3272ece36ce1 | 225 | } else { |
krmreynolds | 1:3272ece36ce1 | 226 | return LSM303_SA0_A_HIGH; |
krmreynolds | 1:3272ece36ce1 | 227 | } |
krmreynolds | 0:dc35364e2291 | 228 | } |