Hiroshi Yamaguchi / LSM303DLH

LSM303DLH interface library

Dependents:   LSM303DLH_Example Arch_Test

LSM303DLH.cpp@0:9b2a0b783bfc, 2011-10-06 (annotated)

Committer:
yamaguch
Date:
Thu Oct 06 08:38:06 2011 +0000
Revision:
0:9b2a0b783bfc
0.1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yamaguch 0:9b2a0b783bfc 1 #include "LSM303DLH.h"
yamaguch 0:9b2a0b783bfc 2 #include "Vector.h"
yamaguch 0:9b2a0b783bfc 3
yamaguch 0:9b2a0b783bfc 4 const char SAD_ACC = 0x30;
yamaguch 0:9b2a0b783bfc 5 const char SAD_MAG = 0x3C;
yamaguch 0:9b2a0b783bfc 6 const float ACC_UNIT[] = {0.001, 0.002, 0, 0.0039};
yamaguch 0:9b2a0b783bfc 7 const float MAG_UNIT_XY[] = {0, 1.0 / 1055, 1.0 / 795, 1.0 / 635, 1.0 / 430, 1.0 / 375, 1.0 / 320, 1.0 / 230};
yamaguch 0:9b2a0b783bfc 8 const float MAG_UNIT_Z[] = {0, 1.0 / 950, 1.0 / 710, 1.0 / 570, 1.0 / 385, 1.0 / 335, 1.0 / 285, 1.0 / 205};
yamaguch 0:9b2a0b783bfc 9
yamaguch 0:9b2a0b783bfc 10 enum REG_ADDRS {
yamaguch 0:9b2a0b783bfc 11 CRA_REG_M = 0x00,
yamaguch 0:9b2a0b783bfc 12 CRB_REG_M = 0x01,
yamaguch 0:9b2a0b783bfc 13 MR_REG_M = 0x02,
yamaguch 0:9b2a0b783bfc 14 OUT_X_H_M = 0x03,
yamaguch 0:9b2a0b783bfc 15 OUT_X_L_M = 0x04,
yamaguch 0:9b2a0b783bfc 16 OUT_Y_H_M = 0x05,
yamaguch 0:9b2a0b783bfc 17 OUT_Y_L_M = 0x06,
yamaguch 0:9b2a0b783bfc 18 OUT_Z_H_M = 0x07,
yamaguch 0:9b2a0b783bfc 19 OUT_Z_L_M = 0x08,
yamaguch 0:9b2a0b783bfc 20 SR_REG_M = 0x09,
yamaguch 0:9b2a0b783bfc 21 IRA_REG_M = 0x0A,
yamaguch 0:9b2a0b783bfc 22 IRB_REG_M = 0x0B,
yamaguch 0:9b2a0b783bfc 23 IRC_REG_M = 0x0C,
yamaguch 0:9b2a0b783bfc 24
yamaguch 0:9b2a0b783bfc 25 CTRL_REG1_A = 0x20,
yamaguch 0:9b2a0b783bfc 26 CTRL_REG2_A = 0x21,
yamaguch 0:9b2a0b783bfc 27 CTRL_REG3_A = 0x22,
yamaguch 0:9b2a0b783bfc 28 CTRL_REG4_A = 0x23,
yamaguch 0:9b2a0b783bfc 29 CTRL_REG5_A = 0x24,
yamaguch 0:9b2a0b783bfc 30 HP_FILTER_RESET_A = 0x25,
yamaguch 0:9b2a0b783bfc 31 REFERENCE_A = 0x26,
yamaguch 0:9b2a0b783bfc 32 STATUS_REG_A = 0x27,
yamaguch 0:9b2a0b783bfc 33 OUT_X_L_A = 0x28,
yamaguch 0:9b2a0b783bfc 34 OUT_X_H_A = 0x29,
yamaguch 0:9b2a0b783bfc 35 OUT_Y_L_A = 0x2A,
yamaguch 0:9b2a0b783bfc 36 OUT_Y_H_A = 0x2B,
yamaguch 0:9b2a0b783bfc 37 OUT_Z_L_A = 0x2C,
yamaguch 0:9b2a0b783bfc 38 OUT_Z_H_A = 0x2D,
yamaguch 0:9b2a0b783bfc 39
yamaguch 0:9b2a0b783bfc 40 INT1_CFG_A = 0x30,
yamaguch 0:9b2a0b783bfc 41 INT1_SOURCE_A = 0x31,
yamaguch 0:9b2a0b783bfc 42 INT1_THS_A = 0x32,
yamaguch 0:9b2a0b783bfc 43 INT1_DURATION_A = 0x33,
yamaguch 0:9b2a0b783bfc 44 INT2_CFG_A = 0x34,
yamaguch 0:9b2a0b783bfc 45 INT2_SOURCE_A = 0x35,
yamaguch 0:9b2a0b783bfc 46 INT2_THS_A = 0x36,
yamaguch 0:9b2a0b783bfc 47 INT2_DURATION_A = 0x37,
yamaguch 0:9b2a0b783bfc 48 };
yamaguch 0:9b2a0b783bfc 49
yamaguch 0:9b2a0b783bfc 50 LSM303DLH::LSM303DLH(PinName sda, PinName scl, PinName int1, PinName int2)
yamaguch 0:9b2a0b783bfc 51 : ax(0), ay(0), az(0), mx(0), my(0), mz(0),
yamaguch 0:9b2a0b783bfc 52 i2c(sda, scl), acc_range(ACC_RANGE_2G), mag_range(MAG_RANGE_1_3GAUSS),
yamaguch 0:9b2a0b783bfc 53 int1(int1), int2(int2) {
yamaguch 0:9b2a0b783bfc 54 send(SAD_ACC, CTRL_REG1_A, 0x27); // output rate 400 Hz
yamaguch 0:9b2a0b783bfc 55 send(SAD_ACC, CTRL_REG4_A, 0xC0); // block data update; big endian, max=2g
yamaguch 0:9b2a0b783bfc 56 send(SAD_MAG, CRA_REG_M, 0x18); // min data output rate 75Hz
yamaguch 0:9b2a0b783bfc 57 send(SAD_MAG, MR_REG_M, 0x00); // continuous conversion mode
yamaguch 0:9b2a0b783bfc 58 }
yamaguch 0:9b2a0b783bfc 59
yamaguch 0:9b2a0b783bfc 60 bool LSM303DLH::setOutputDataRate(ACC_ODR acc_odr, MAG_ODR mag_odr) {
yamaguch 0:9b2a0b783bfc 61 return send(SAD_ACC, CTRL_REG4_A, 0x27 | acc_odr << 3) &&
yamaguch 0:9b2a0b783bfc 62 send(SAD_MAG, CRA_REG_M, mag_odr << 2);
yamaguch 0:9b2a0b783bfc 63 }
yamaguch 0:9b2a0b783bfc 64
yamaguch 0:9b2a0b783bfc 65 bool LSM303DLH::setMeasurementRange(ACC_RANGE acc_range, MAG_RANGE mag_range) {
yamaguch 0:9b2a0b783bfc 66 this->acc_range = acc_range;
yamaguch 0:9b2a0b783bfc 67 this->mag_range = mag_range;
yamaguch 0:9b2a0b783bfc 68
yamaguch 0:9b2a0b783bfc 69 return send(SAD_ACC, CTRL_REG4_A, 0xC0 | acc_range << 4) &&
yamaguch 0:9b2a0b783bfc 70 send(SAD_MAG, CRB_REG_M, mag_range << 5);
yamaguch 0:9b2a0b783bfc 71 }
yamaguch 0:9b2a0b783bfc 72
yamaguch 0:9b2a0b783bfc 73 bool LSM303DLH::read() {
yamaguch 0:9b2a0b783bfc 74 char acc[6], mag[6];
yamaguch 0:9b2a0b783bfc 75
yamaguch 0:9b2a0b783bfc 76 if (recv(0x30, OUT_X_L_A, acc, 6) && recv(0x3C, OUT_X_H_M, mag, 6)) {
yamaguch 0:9b2a0b783bfc 77 ax = ACC_UNIT[acc_range] * short(acc[0] << 8 | acc[1]) / 16;
yamaguch 0:9b2a0b783bfc 78 ay = ACC_UNIT[acc_range] * short(acc[2] << 8 | acc[3]) / 16;
yamaguch 0:9b2a0b783bfc 79 az = ACC_UNIT[acc_range] * short(acc[4] << 8 | acc[5]) / 16;
yamaguch 0:9b2a0b783bfc 80 mx = MAG_UNIT_XY[mag_range] * short(mag[0] << 8 | mag[1]);
yamaguch 0:9b2a0b783bfc 81 my = MAG_UNIT_XY[mag_range] * short(mag[2] << 8 | mag[3]);
yamaguch 0:9b2a0b783bfc 82 mz = MAG_UNIT_Z[mag_range] * short(mag[4] << 8 | mag[5]);
yamaguch 0:9b2a0b783bfc 83
yamaguch 0:9b2a0b783bfc 84 return true;
yamaguch 0:9b2a0b783bfc 85 }
yamaguch 0:9b2a0b783bfc 86
yamaguch 0:9b2a0b783bfc 87 return false;
yamaguch 0:9b2a0b783bfc 88 }
yamaguch 0:9b2a0b783bfc 89
yamaguch 0:9b2a0b783bfc 90 bool LSM303DLH::read(float *ax, float *ay, float *az, float *mx, float *my, float *mz) {
yamaguch 0:9b2a0b783bfc 91 if (ax != 0 || ay != 0 || az != 0) {
yamaguch 0:9b2a0b783bfc 92 char acc[6];
yamaguch 0:9b2a0b783bfc 93
yamaguch 0:9b2a0b783bfc 94 if (recv(0x30, OUT_X_L_A, acc, 6)) {
yamaguch 0:9b2a0b783bfc 95 if (ax != 0) *ax = ACC_UNIT[acc_range] * short(acc[0] << 8 | acc[1]) / 16;
yamaguch 0:9b2a0b783bfc 96 if (ay != 0) *ay = ACC_UNIT[acc_range] * short(acc[2] << 8 | acc[3]) / 16;
yamaguch 0:9b2a0b783bfc 97 if (az != 0) *az = ACC_UNIT[acc_range] * short(acc[4] << 8 | acc[5]) / 16;
yamaguch 0:9b2a0b783bfc 98 } else
yamaguch 0:9b2a0b783bfc 99 return false;
yamaguch 0:9b2a0b783bfc 100 }
yamaguch 0:9b2a0b783bfc 101
yamaguch 0:9b2a0b783bfc 102 if (mx != 0 || my != 0 || mz != 0) {
yamaguch 0:9b2a0b783bfc 103 char mag[6];
yamaguch 0:9b2a0b783bfc 104
yamaguch 0:9b2a0b783bfc 105 if (recv(0x3C, OUT_X_H_M, mag, 6)) {
yamaguch 0:9b2a0b783bfc 106 if (mx != 0) *mx = MAG_UNIT_XY[mag_range] * short(mag[0] << 8 | mag[1]);
yamaguch 0:9b2a0b783bfc 107 if (my != 0) *my = MAG_UNIT_XY[mag_range] * short(mag[2] << 8 | mag[3]);
yamaguch 0:9b2a0b783bfc 108 if (mz != 0) *mz = MAG_UNIT_Z[mag_range] * short(mag[4] << 8 | mag[5]);
yamaguch 0:9b2a0b783bfc 109 } else
yamaguch 0:9b2a0b783bfc 110 return false;
yamaguch 0:9b2a0b783bfc 111 }
yamaguch 0:9b2a0b783bfc 112
yamaguch 0:9b2a0b783bfc 113 return true;
yamaguch 0:9b2a0b783bfc 114 }
yamaguch 0:9b2a0b783bfc 115
yamaguch 0:9b2a0b783bfc 116 float LSM303DLH::getHeading() {
yamaguch 0:9b2a0b783bfc 117 return getHeading(0, 1, 0);
yamaguch 0:9b2a0b783bfc 118 }
yamaguch 0:9b2a0b783bfc 119
yamaguch 0:9b2a0b783bfc 120 float LSM303DLH::getHeading(float x, float y, float z) {
yamaguch 0:9b2a0b783bfc 121 Vector base(x, y, z);
yamaguch 0:9b2a0b783bfc 122 Vector accel = Vector(ax, ay, az);
yamaguch 0:9b2a0b783bfc 123 Vector compass = Vector(mx, my, mz);
yamaguch 0:9b2a0b783bfc 124 Vector east = compass * accel;
yamaguch 0:9b2a0b783bfc 125 Vector north = accel * east;
yamaguch 0:9b2a0b783bfc 126 east /= east.norm();
yamaguch 0:9b2a0b783bfc 127 north /= north.norm();
yamaguch 0:9b2a0b783bfc 128 //printf("accel = (%.1f, %.1f, %.1f), compass = (%.1f, %.1f, %.1f)\n", accel.x, accel.y, accel.z, compass.x, compass.y, compass.z);
yamaguch 0:9b2a0b783bfc 129 //printf("north = (%.1f, %.1f, %.1f), east = (%.1f, %.1f, %.1f)\n", north.x, north.y, north.z, east.x, east.y, east.z);
yamaguch 0:9b2a0b783bfc 130 //printf("east.dot(base) = %.1f, north.dot(base) = %.1f\n", east.dot(base), north.dot(base));
yamaguch 0:9b2a0b783bfc 131
yamaguch 0:9b2a0b783bfc 132 return atan2(east.dot(base), north.dot(base));
yamaguch 0:9b2a0b783bfc 133 }
yamaguch 0:9b2a0b783bfc 134
yamaguch 0:9b2a0b783bfc 135 void LSM303DLH::attach(INT_TYPE type, char config, char threshold, char duration, void (*handler)(void)) {
yamaguch 0:9b2a0b783bfc 136 InterruptIn& interruptIn = (type == INT1) ? int1 : int2;
yamaguch 0:9b2a0b783bfc 137 send(SAD_ACC, type == INT1 ? INT1_CFG_A : INT2_CFG_A, config);
yamaguch 0:9b2a0b783bfc 138 send(SAD_ACC, type == INT1 ? INT1_THS_A : INT2_THS_A, threshold);
yamaguch 0:9b2a0b783bfc 139 send(SAD_ACC, type == INT1 ? INT1_DURATION_A : INT2_DURATION_A, duration);
yamaguch 0:9b2a0b783bfc 140 interruptIn.rise(handler);
yamaguch 0:9b2a0b783bfc 141 }
yamaguch 0:9b2a0b783bfc 142
yamaguch 0:9b2a0b783bfc 143 template<typename T> void LSM303DLH::attach(T *t, INT_TYPE type, char config, char threshold, char duration, void (*handler)(void)) {
yamaguch 0:9b2a0b783bfc 144 InterruptIn& interruptIn = (type == INT1) ? int1 : int2;
yamaguch 0:9b2a0b783bfc 145 interruptIn.rise(t, handler);
yamaguch 0:9b2a0b783bfc 146 }
yamaguch 0:9b2a0b783bfc 147
yamaguch 0:9b2a0b783bfc 148 bool LSM303DLH::recv(char sad, char sub, char *buf, int length) {
yamaguch 0:9b2a0b783bfc 149 if (length > 1) sub |= 0x80;
yamaguch 0:9b2a0b783bfc 150
yamaguch 0:9b2a0b783bfc 151 return i2c.write(sad, &sub, 1, true) == 0 && i2c.read(sad, buf, length) == 0;
yamaguch 0:9b2a0b783bfc 152 }
yamaguch 0:9b2a0b783bfc 153
yamaguch 0:9b2a0b783bfc 154 bool LSM303DLH::send(char sad, char sub, char data) {
yamaguch 0:9b2a0b783bfc 155 char buf[] = {sub, data};
yamaguch 0:9b2a0b783bfc 156
yamaguch 0:9b2a0b783bfc 157 return i2c.write(sad, buf, 2) == 0;
yamaguch 0:9b2a0b783bfc 158 }