library for using LSM303DM chip
LSM303.cpp@0:4d358fbeab6e, 2013-10-10 (annotated)
- Committer:
- fin4478
- Date:
- Thu Oct 10 06:57:53 2013 +0000
- Revision:
- 0:4d358fbeab6e
- Child:
- 1:322c80f884d3
initial version
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 | 0:4d358fbeab6e | 5 | I2C i2c(P0_5, P0_4); |
fin4478 | 0:4d358fbeab6e | 6 | |
fin4478 | 0:4d358fbeab6e | 7 | int LSM303::setup() |
fin4478 | 0:4d358fbeab6e | 8 | { |
fin4478 | 0:4d358fbeab6e | 9 | #if 1 |
fin4478 | 0:4d358fbeab6e | 10 | m_max.x = 239; |
fin4478 | 0:4d358fbeab6e | 11 | m_max.y = 242; |
fin4478 | 0:4d358fbeab6e | 12 | m_max.z = 158; |
fin4478 | 0:4d358fbeab6e | 13 | m_min.x = -637; |
fin4478 | 0:4d358fbeab6e | 14 | m_min.y = -498; |
fin4478 | 0:4d358fbeab6e | 15 | m_min.z = -538; |
fin4478 | 0:4d358fbeab6e | 16 | #else |
fin4478 | 0:4d358fbeab6e | 17 | m_max.x = 1; |
fin4478 | 0:4d358fbeab6e | 18 | m_max.y = 1; |
fin4478 | 0:4d358fbeab6e | 19 | m_max.z = 1; |
fin4478 | 0:4d358fbeab6e | 20 | m_min.x = 0; |
fin4478 | 0:4d358fbeab6e | 21 | m_min.y = 0; |
fin4478 | 0:4d358fbeab6e | 22 | m_min.z = 0; |
fin4478 | 0:4d358fbeab6e | 23 | #endif |
fin4478 | 0:4d358fbeab6e | 24 | return initLSM303(ACCELE_SCALE); // Initialize the LSM303, using a SCALE full-scale range |
fin4478 | 0:4d358fbeab6e | 25 | } |
fin4478 | 0:4d358fbeab6e | 26 | int LSM303::testAcc() |
fin4478 | 0:4d358fbeab6e | 27 | { |
fin4478 | 0:4d358fbeab6e | 28 | if (i2c.write(LSM303_ACC, NULL, 0) ==0) return LSM303_ACC; |
fin4478 | 0:4d358fbeab6e | 29 | return 255; |
fin4478 | 0:4d358fbeab6e | 30 | } |
fin4478 | 0:4d358fbeab6e | 31 | |
fin4478 | 0:4d358fbeab6e | 32 | int LSM303::testMag() |
fin4478 | 0:4d358fbeab6e | 33 | { |
fin4478 | 0:4d358fbeab6e | 34 | if (i2c.write(LSM303_MAG, NULL, 0) ==0) |
fin4478 | 0:4d358fbeab6e | 35 | if (LSM303_read(LSM303_WHO_AM_I_M)==0x3C) { |
fin4478 | 0:4d358fbeab6e | 36 | return LSM303_WHO_AM_I_M; |
fin4478 | 0:4d358fbeab6e | 37 | } |
fin4478 | 0:4d358fbeab6e | 38 | else { |
fin4478 | 0:4d358fbeab6e | 39 | return LSM303_MAG; |
fin4478 | 0:4d358fbeab6e | 40 | } |
fin4478 | 0:4d358fbeab6e | 41 | |
fin4478 | 0:4d358fbeab6e | 42 | return 255; |
fin4478 | 0:4d358fbeab6e | 43 | } |
fin4478 | 0:4d358fbeab6e | 44 | |
fin4478 | 0:4d358fbeab6e | 45 | int LSM303::initLSM303(int fs) |
fin4478 | 0:4d358fbeab6e | 46 | { |
fin4478 | 0:4d358fbeab6e | 47 | if (LSM303_write(0x27, CTRL_REG1_A) == 1) { // 0x27 = normal power mode, all accel axes on |
fin4478 | 0:4d358fbeab6e | 48 | //LSM303_write(16, CTRL_REG2_A); // enable internal hp filter |
fin4478 | 0:4d358fbeab6e | 49 | } |
fin4478 | 0:4d358fbeab6e | 50 | if ((fs==8)||(fs==4)) |
fin4478 | 0:4d358fbeab6e | 51 | LSM303_write((0x00 | (fs-fs/2-1)<<4), CTRL_REG4_A); // set full-scale |
fin4478 | 0:4d358fbeab6e | 52 | else |
fin4478 | 0:4d358fbeab6e | 53 | LSM303_write(0x00, CTRL_REG4_A); |
fin4478 | 0:4d358fbeab6e | 54 | //LSM303_write(20, CRA_REG_M); // 20 = mag 30Hz output rate |
fin4478 | 0:4d358fbeab6e | 55 | LSM303_write(MAG_SCALE_1_3, CRB_REG_M); //magnetic scale = +/-1.3Gauss |
fin4478 | 0:4d358fbeab6e | 56 | LSM303_write(0x00, MR_REG_M); // 0x00 = continouous conversion mode |
fin4478 | 0:4d358fbeab6e | 57 | |
fin4478 | 0:4d358fbeab6e | 58 | return 1; |
fin4478 | 0:4d358fbeab6e | 59 | } |
fin4478 | 0:4d358fbeab6e | 60 | |
fin4478 | 0:4d358fbeab6e | 61 | float LSM303::getTiltHeading() |
fin4478 | 0:4d358fbeab6e | 62 | { |
fin4478 | 0:4d358fbeab6e | 63 | //shift and scale |
fin4478 | 0:4d358fbeab6e | 64 | a.x = a.x / 32768 * ACCELE_SCALE; |
fin4478 | 0:4d358fbeab6e | 65 | a.y = a.y / 32768 * ACCELE_SCALE; |
fin4478 | 0:4d358fbeab6e | 66 | a.z = a.z / 32768 * ACCELE_SCALE; |
fin4478 | 0:4d358fbeab6e | 67 | |
fin4478 | 0:4d358fbeab6e | 68 | m.x = (m.x - m_min.x) / (m_max.x - m_min.x) * 2 - 1.0; |
fin4478 | 0:4d358fbeab6e | 69 | m.y = (m.y - m_min.y) / (m_max.y - m_min.y) * 2 - 1.0; |
fin4478 | 0:4d358fbeab6e | 70 | m.z = (m.z - m_min.z) / (m_max.z - m_min.z) * 2 - 1.0; |
fin4478 | 0:4d358fbeab6e | 71 | |
fin4478 | 0:4d358fbeab6e | 72 | vector_normalize(&a); |
fin4478 | 0:4d358fbeab6e | 73 | vector_normalize(&m); |
fin4478 | 0:4d358fbeab6e | 74 | //see appendix A in app note AN3192 |
fin4478 | 0:4d358fbeab6e | 75 | pitch = asin(-a.x); |
fin4478 | 0:4d358fbeab6e | 76 | roll = asin(a.y/cos(pitch)); |
fin4478 | 0:4d358fbeab6e | 77 | float heading = 0; |
fin4478 | 0:4d358fbeab6e | 78 | float xh = m.x * cos(pitch) + m.z * sin(pitch); |
fin4478 | 0:4d358fbeab6e | 79 | float yh = m.x * sin(roll) * sin(pitch) + m.y * cos(roll) - m.z * sin(roll) * cos(pitch); |
fin4478 | 0:4d358fbeab6e | 80 | //float zh = -m.x * cos(roll) * sin(pitch) + m.y * sin(roll) + m.z * cos(roll) * cos(pitch); |
fin4478 | 0:4d358fbeab6e | 81 | heading = 180 * atan2(yh, xh)/PI; |
fin4478 | 0:4d358fbeab6e | 82 | if (heading < 0) heading += 360; |
fin4478 | 0:4d358fbeab6e | 83 | return heading; |
fin4478 | 0:4d358fbeab6e | 84 | } |
fin4478 | 0:4d358fbeab6e | 85 | |
fin4478 | 0:4d358fbeab6e | 86 | void LSM303::vector_cross( const Plane *a,const Plane *b, Plane *out ) |
fin4478 | 0:4d358fbeab6e | 87 | { |
fin4478 | 0:4d358fbeab6e | 88 | out->x = a->y*b->z - a->z*b->y; |
fin4478 | 0:4d358fbeab6e | 89 | out->y = a->z*b->x - a->x*b->z; |
fin4478 | 0:4d358fbeab6e | 90 | out->z = a->x*b->y - a->y*b->x; |
fin4478 | 0:4d358fbeab6e | 91 | } |
fin4478 | 0:4d358fbeab6e | 92 | |
fin4478 | 0:4d358fbeab6e | 93 | float LSM303::vector_dot( const Plane *a,const Plane *b ) |
fin4478 | 0:4d358fbeab6e | 94 | { |
fin4478 | 0:4d358fbeab6e | 95 | return a->x*b->x+a->y*b->y+a->z*b->z; |
fin4478 | 0:4d358fbeab6e | 96 | } |
fin4478 | 0:4d358fbeab6e | 97 | |
fin4478 | 0:4d358fbeab6e | 98 | void LSM303::vector_normalize( Plane *a ) |
fin4478 | 0:4d358fbeab6e | 99 | { |
fin4478 | 0:4d358fbeab6e | 100 | float mag = sqrt(vector_dot(a,a)); |
fin4478 | 0:4d358fbeab6e | 101 | a->x /= mag; |
fin4478 | 0:4d358fbeab6e | 102 | a->y /= mag; |
fin4478 | 0:4d358fbeab6e | 103 | a->z /= mag; |
fin4478 | 0:4d358fbeab6e | 104 | } |
fin4478 | 0:4d358fbeab6e | 105 | |
fin4478 | 0:4d358fbeab6e | 106 | int LSM303::getLSM303_accel() |
fin4478 | 0:4d358fbeab6e | 107 | { |
fin4478 | 0:4d358fbeab6e | 108 | char data[1] = { OUT_X_L_A | (1<<7)}; |
fin4478 | 0:4d358fbeab6e | 109 | char out[6] = {0,0,0,0,0,0}; |
fin4478 | 0:4d358fbeab6e | 110 | i2c.write( LSM303_ACC, data,1); |
fin4478 | 0:4d358fbeab6e | 111 | i2c.read( LSM303_ACC, out, 6); |
fin4478 | 0:4d358fbeab6e | 112 | |
fin4478 | 0:4d358fbeab6e | 113 | a.x = short( (((short)out[1]) << 8) | out[0] ); |
fin4478 | 0:4d358fbeab6e | 114 | a.y = short( (((short)out[3]) << 8) | out[2] ); |
fin4478 | 0:4d358fbeab6e | 115 | a.z = short( (((short)out[5]) << 8) | out[4] ); |
fin4478 | 0:4d358fbeab6e | 116 | return 0; |
fin4478 | 0:4d358fbeab6e | 117 | } |
fin4478 | 0:4d358fbeab6e | 118 | |
fin4478 | 0:4d358fbeab6e | 119 | void LSM303::getLSM303_mag() |
fin4478 | 0:4d358fbeab6e | 120 | { |
fin4478 | 0:4d358fbeab6e | 121 | char data[1] = { OUT_X_H_M }; |
fin4478 | 0:4d358fbeab6e | 122 | char out[6]; |
fin4478 | 0:4d358fbeab6e | 123 | |
fin4478 | 0:4d358fbeab6e | 124 | i2c.write( LSM303_MAG, data, 1 ); |
fin4478 | 0:4d358fbeab6e | 125 | i2c.read( LSM303_MAG, out, 6 ); |
fin4478 | 0:4d358fbeab6e | 126 | // DLM, DLHC: register address for Z comes before Y |
fin4478 | 0:4d358fbeab6e | 127 | m.x = short( out[0] << 8 | out[1] ); |
fin4478 | 0:4d358fbeab6e | 128 | m.y = short( out[4] << 8 | out[5] ); |
fin4478 | 0:4d358fbeab6e | 129 | m.z= short( out[2] << 8 | out[3] ); |
fin4478 | 0:4d358fbeab6e | 130 | } |
fin4478 | 0:4d358fbeab6e | 131 | |
fin4478 | 0:4d358fbeab6e | 132 | int LSM303::LSM303_read(int address) |
fin4478 | 0:4d358fbeab6e | 133 | { |
fin4478 | 0:4d358fbeab6e | 134 | if (address >= 0x20) { |
fin4478 | 0:4d358fbeab6e | 135 | _i2c_address = LSM303_ACC; |
fin4478 | 0:4d358fbeab6e | 136 | } else { |
fin4478 | 0:4d358fbeab6e | 137 | _i2c_address = LSM303_MAG; |
fin4478 | 0:4d358fbeab6e | 138 | } |
fin4478 | 0:4d358fbeab6e | 139 | |
fin4478 | 0:4d358fbeab6e | 140 | char value[1]; |
fin4478 | 0:4d358fbeab6e | 141 | |
fin4478 | 0:4d358fbeab6e | 142 | char data[1] = { address }; |
fin4478 | 0:4d358fbeab6e | 143 | i2c.write( _i2c_address, data, 1 ); |
fin4478 | 0:4d358fbeab6e | 144 | i2c.read( _i2c_address, value, 1 ); |
fin4478 | 0:4d358fbeab6e | 145 | return value[0]; |
fin4478 | 0:4d358fbeab6e | 146 | } |
fin4478 | 0:4d358fbeab6e | 147 | |
fin4478 | 0:4d358fbeab6e | 148 | int LSM303::LSM303_write(int data, int address) |
fin4478 | 0:4d358fbeab6e | 149 | { |
fin4478 | 0:4d358fbeab6e | 150 | if (address >= 0x20) { |
fin4478 | 0:4d358fbeab6e | 151 | _i2c_address = LSM303_ACC; |
fin4478 | 0:4d358fbeab6e | 152 | } else { |
fin4478 | 0:4d358fbeab6e | 153 | _i2c_address = LSM303_MAG; |
fin4478 | 0:4d358fbeab6e | 154 | } |
fin4478 | 0:4d358fbeab6e | 155 | |
fin4478 | 0:4d358fbeab6e | 156 | char out[2] = { address, data }; |
fin4478 | 0:4d358fbeab6e | 157 | i2c.write( _i2c_address, out, 2 ); |
fin4478 | 0:4d358fbeab6e | 158 | return 0; |
fin4478 | 0:4d358fbeab6e | 159 | } |