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