A Team / DCM_AHRS

DCM Code ported from Arduino for FRDM-KL25Z

Dependents:   minimu_data_capture minimu_data_capture

Fork of DCM_AHRS by Kris Reynolds

LSM303.cpp@2:85214374e094, 2015-01-20 (annotated)

Committer:
ogarai
Date:
Tue Jan 20 02:04:07 2015 +0000
Revision:
2:85214374e094
Parent:
1:3272ece36ce1 
First Commit

Who changed what in which revision?

UserRevisionLine numberNew 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
ogarai 2:85214374e094 5 LSM303::LSM303( PinName sda = PTE0, PinName scl = PTE1 ) : 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 }