LSM303DLH.cpp

00001 #include "LSM303DLH.h"
00002 #include "Vector.h"
00003 
00004 const char SAD_ACC = 0x30;
00005 const char SAD_MAG = 0x3C;
00006 const float ACC_UNIT[] = {0.001, 0.002, 0,  0.0039};
00007 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};
00008 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};
00009 
00010 enum REG_ADDRS {
00011     CRA_REG_M         = 0x00,
00012     CRB_REG_M         = 0x01,
00013     MR_REG_M          = 0x02,
00014     OUT_X_H_M         = 0x03,
00015     OUT_X_L_M         = 0x04,
00016     OUT_Y_H_M         = 0x05,
00017     OUT_Y_L_M         = 0x06,
00018     OUT_Z_H_M         = 0x07,
00019     OUT_Z_L_M         = 0x08,
00020     SR_REG_M          = 0x09,
00021     IRA_REG_M         = 0x0A,
00022     IRB_REG_M         = 0x0B,
00023     IRC_REG_M         = 0x0C,
00024 
00025     CTRL_REG1_A       = 0x20,
00026     CTRL_REG2_A       = 0x21,
00027     CTRL_REG3_A       = 0x22,
00028     CTRL_REG4_A       = 0x23,
00029     CTRL_REG5_A       = 0x24,
00030     HP_FILTER_RESET_A = 0x25,
00031     REFERENCE_A       = 0x26,
00032     STATUS_REG_A      = 0x27,
00033     OUT_X_L_A         = 0x28,
00034     OUT_X_H_A         = 0x29,
00035     OUT_Y_L_A         = 0x2A,
00036     OUT_Y_H_A         = 0x2B,
00037     OUT_Z_L_A         = 0x2C,
00038     OUT_Z_H_A         = 0x2D,
00039 
00040     INT1_CFG_A        = 0x30,
00041     INT1_SOURCE_A     = 0x31,
00042     INT1_THS_A        = 0x32,
00043     INT1_DURATION_A   = 0x33,
00044     INT2_CFG_A        = 0x34,
00045     INT2_SOURCE_A     = 0x35,
00046     INT2_THS_A        = 0x36,
00047     INT2_DURATION_A   = 0x37,
00048 };
00049 
00050 LSM303DLH::LSM303DLH(PinName sda, PinName scl, PinName int1, PinName int2)
00051         : ax(0), ay(0), az(0), mx(0), my(0), mz(0),
00052         i2c(sda, scl), acc_range(ACC_RANGE_2G), mag_range(MAG_RANGE_1_3GAUSS),
00053         int1(int1), int2(int2) {
00054     send(SAD_ACC, CTRL_REG1_A, 0x27); // output rate 400 Hz
00055     send(SAD_ACC, CTRL_REG4_A, 0xC0); // block data update; big endian, max=2g
00056     send(SAD_MAG, CRA_REG_M, 0x18); // min data output rate 75Hz
00057     send(SAD_MAG, MR_REG_M, 0x00); // continuous conversion mode
00058 }
00059 
00060 bool LSM303DLH::setOutputDataRate(ACC_ODR acc_odr, MAG_ODR mag_odr) {
00061     return send(SAD_ACC, CTRL_REG4_A, 0x27 | acc_odr << 3) &&
00062            send(SAD_MAG, CRA_REG_M, mag_odr << 2);
00063 }
00064 
00065 bool LSM303DLH::setMeasurementRange(ACC_RANGE acc_range, MAG_RANGE mag_range) {
00066     this->acc_range = acc_range;
00067     this->mag_range = mag_range;
00068 
00069     return send(SAD_ACC, CTRL_REG4_A, 0xC0 | acc_range << 4) &&
00070            send(SAD_MAG, CRB_REG_M, mag_range << 5);
00071 }
00072 
00073 bool LSM303DLH::read() {
00074     char acc[6], mag[6];
00075 
00076     if (recv(0x30, OUT_X_L_A, acc, 6) && recv(0x3C, OUT_X_H_M, mag, 6)) {
00077         ax = ACC_UNIT[acc_range] * short(acc[0] << 8 | acc[1]) / 16;
00078         ay = ACC_UNIT[acc_range] * short(acc[2] << 8 | acc[3]) / 16;
00079         az = ACC_UNIT[acc_range] * short(acc[4] << 8 | acc[5]) / 16;
00080         mx = MAG_UNIT_XY[mag_range] * short(mag[0] << 8 | mag[1]);
00081         my = MAG_UNIT_XY[mag_range] * short(mag[2] << 8 | mag[3]);
00082         mz = MAG_UNIT_Z[mag_range] * short(mag[4] << 8 | mag[5]);
00083 
00084         return true;
00085     }
00086 
00087     return false;
00088 }
00089 
00090 bool LSM303DLH::read(float *ax, float *ay, float *az, float *mx, float *my, float *mz) {
00091     if (ax != 0 || ay != 0 || az != 0) {
00092         char acc[6];
00093 
00094         if (recv(0x30, OUT_X_L_A, acc, 6)) {
00095             if (ax != 0) *ax = ACC_UNIT[acc_range] * short(acc[0] << 8 | acc[1]) / 16;
00096             if (ay != 0) *ay = ACC_UNIT[acc_range] * short(acc[2] << 8 | acc[3]) / 16;
00097             if (az != 0) *az = ACC_UNIT[acc_range] * short(acc[4] << 8 | acc[5]) / 16;
00098         } else
00099             return false;
00100     }
00101 
00102     if (mx != 0 || my != 0 || mz != 0) {
00103         char mag[6];
00104 
00105         if (recv(0x3C, OUT_X_H_M, mag, 6)) {
00106             if (mx != 0) *mx = MAG_UNIT_XY[mag_range] * short(mag[0] << 8 | mag[1]);
00107             if (my != 0) *my = MAG_UNIT_XY[mag_range] * short(mag[2] << 8 | mag[3]);
00108             if (mz != 0) *mz = MAG_UNIT_Z[mag_range] * short(mag[4] << 8 | mag[5]);
00109         } else
00110             return false;
00111     }
00112 
00113     return true;
00114 }
00115 
00116 float LSM303DLH::getHeading() {
00117     return getHeading(0, 1, 0);
00118 }
00119 
00120 float LSM303DLH::getHeading(float x, float y, float z) {
00121     Vector base(x, y, z);
00122     Vector accel = Vector(ax, ay, az);
00123     Vector compass = Vector(mx, my, mz);
00124     Vector east = compass * accel;
00125     Vector north = accel * east;
00126     east /= east.norm();
00127     north /= north.norm();
00128     //printf("accel = (%.1f, %.1f, %.1f), compass = (%.1f, %.1f, %.1f)\n", accel.x, accel.y, accel.z, compass.x, compass.y, compass.z);
00129     //printf("north = (%.1f, %.1f, %.1f), east = (%.1f, %.1f, %.1f)\n", north.x, north.y, north.z, east.x, east.y, east.z);
00130     //printf("east.dot(base) = %.1f, north.dot(base) = %.1f\n", east.dot(base), north.dot(base));
00131 
00132     return atan2(east.dot(base), north.dot(base));
00133 }
00134 
00135 void LSM303DLH::attach(INT_TYPE type, char config, char threshold, char duration, void (*handler)(void)) {
00136     InterruptIn& interruptIn = (type == INT1) ? int1 : int2;
00137     send(SAD_ACC, type == INT1 ? INT1_CFG_A : INT2_CFG_A, config);
00138     send(SAD_ACC, type == INT1 ? INT1_THS_A : INT2_THS_A, threshold);
00139     send(SAD_ACC, type == INT1 ? INT1_DURATION_A : INT2_DURATION_A, duration);
00140     interruptIn.rise(handler);
00141 }
00142 
00143 template<typename T> void LSM303DLH::attach(T *t, INT_TYPE type, char config, char threshold, char duration, void (*handler)(void)) {
00144     InterruptIn& interruptIn = (type == INT1) ? int1 : int2;
00145     interruptIn.rise(t, handler);
00146 }
00147 
00148 bool LSM303DLH::recv(char sad, char sub, char *buf, int length) {
00149     if (length > 1) sub |= 0x80;
00150 
00151     return i2c.write(sad, &sub, 1, true) == 0 && i2c.read(sad, buf, length) == 0;
00152 }
00153 
00154 bool LSM303DLH::send(char sad, char sub, char data) {
00155     char buf[] = {sub, data};
00156 
00157     return i2c.write(sad, buf, 2) == 0;
00158 }