Revision 0:9b2a0b783bfc, committed 2011-10-06

Comitter:

yamaguch

Date:

Thu Oct 06 08:38:06 2011 +0000

Commit message:

0.1

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LSM303DLH.cpp	Thu Oct 06 08:38:06 2011 +0000
@@ -0,0 +1,158 @@
+#include "LSM303DLH.h"
+#include "Vector.h"
+
+const char SAD_ACC = 0x30;
+const char SAD_MAG = 0x3C;
+const float ACC_UNIT[] = {0.001, 0.002, 0,  0.0039};
+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};
+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};
+
+enum REG_ADDRS {
+    CRA_REG_M         = 0x00,
+    CRB_REG_M         = 0x01,
+    MR_REG_M          = 0x02,
+    OUT_X_H_M         = 0x03,
+    OUT_X_L_M         = 0x04,
+    OUT_Y_H_M         = 0x05,
+    OUT_Y_L_M         = 0x06,
+    OUT_Z_H_M         = 0x07,
+    OUT_Z_L_M         = 0x08,
+    SR_REG_M          = 0x09,
+    IRA_REG_M         = 0x0A,
+    IRB_REG_M         = 0x0B,
+    IRC_REG_M         = 0x0C,
+
+    CTRL_REG1_A       = 0x20,
+    CTRL_REG2_A       = 0x21,
+    CTRL_REG3_A       = 0x22,
+    CTRL_REG4_A       = 0x23,
+    CTRL_REG5_A       = 0x24,
+    HP_FILTER_RESET_A = 0x25,
+    REFERENCE_A       = 0x26,
+    STATUS_REG_A      = 0x27,
+    OUT_X_L_A         = 0x28,
+    OUT_X_H_A         = 0x29,
+    OUT_Y_L_A         = 0x2A,
+    OUT_Y_H_A         = 0x2B,
+    OUT_Z_L_A         = 0x2C,
+    OUT_Z_H_A         = 0x2D,
+
+    INT1_CFG_A        = 0x30,
+    INT1_SOURCE_A     = 0x31,
+    INT1_THS_A        = 0x32,
+    INT1_DURATION_A   = 0x33,
+    INT2_CFG_A        = 0x34,
+    INT2_SOURCE_A     = 0x35,
+    INT2_THS_A        = 0x36,
+    INT2_DURATION_A   = 0x37,
+};
+
+LSM303DLH::LSM303DLH(PinName sda, PinName scl, PinName int1, PinName int2)
+        : ax(0), ay(0), az(0), mx(0), my(0), mz(0),
+        i2c(sda, scl), acc_range(ACC_RANGE_2G), mag_range(MAG_RANGE_1_3GAUSS),
+        int1(int1), int2(int2) {
+    send(SAD_ACC, CTRL_REG1_A, 0x27); // output rate 400 Hz
+    send(SAD_ACC, CTRL_REG4_A, 0xC0); // block data update; big endian, max=2g
+    send(SAD_MAG, CRA_REG_M, 0x18); // min data output rate 75Hz
+    send(SAD_MAG, MR_REG_M, 0x00); // continuous conversion mode
+}
+
+bool LSM303DLH::setOutputDataRate(ACC_ODR acc_odr, MAG_ODR mag_odr) {
+    return send(SAD_ACC, CTRL_REG4_A, 0x27 | acc_odr << 3) &&
+           send(SAD_MAG, CRA_REG_M, mag_odr << 2);
+}
+
+bool LSM303DLH::setMeasurementRange(ACC_RANGE acc_range, MAG_RANGE mag_range) {
+    this->acc_range = acc_range;
+    this->mag_range = mag_range;
+
+    return send(SAD_ACC, CTRL_REG4_A, 0xC0 | acc_range << 4) &&
+           send(SAD_MAG, CRB_REG_M, mag_range << 5);
+}
+
+bool LSM303DLH::read() {
+    char acc[6], mag[6];
+
+    if (recv(0x30, OUT_X_L_A, acc, 6) && recv(0x3C, OUT_X_H_M, mag, 6)) {
+        ax = ACC_UNIT[acc_range] * short(acc[0] << 8 | acc[1]) / 16;
+        ay = ACC_UNIT[acc_range] * short(acc[2] << 8 | acc[3]) / 16;
+        az = ACC_UNIT[acc_range] * short(acc[4] << 8 | acc[5]) / 16;
+        mx = MAG_UNIT_XY[mag_range] * short(mag[0] << 8 | mag[1]);
+        my = MAG_UNIT_XY[mag_range] * short(mag[2] << 8 | mag[3]);
+        mz = MAG_UNIT_Z[mag_range] * short(mag[4] << 8 | mag[5]);
+
+        return true;
+    }
+
+    return false;
+}
+
+bool LSM303DLH::read(float *ax, float *ay, float *az, float *mx, float *my, float *mz) {
+    if (ax != 0 || ay != 0 || az != 0) {
+        char acc[6];
+
+        if (recv(0x30, OUT_X_L_A, acc, 6)) {
+            if (ax != 0) *ax = ACC_UNIT[acc_range] * short(acc[0] << 8 | acc[1]) / 16;
+            if (ay != 0) *ay = ACC_UNIT[acc_range] * short(acc[2] << 8 | acc[3]) / 16;
+            if (az != 0) *az = ACC_UNIT[acc_range] * short(acc[4] << 8 | acc[5]) / 16;
+        } else
+            return false;
+    }
+
+    if (mx != 0 || my != 0 || mz != 0) {
+        char mag[6];
+
+        if (recv(0x3C, OUT_X_H_M, mag, 6)) {
+            if (mx != 0) *mx = MAG_UNIT_XY[mag_range] * short(mag[0] << 8 | mag[1]);
+            if (my != 0) *my = MAG_UNIT_XY[mag_range] * short(mag[2] << 8 | mag[3]);
+            if (mz != 0) *mz = MAG_UNIT_Z[mag_range] * short(mag[4] << 8 | mag[5]);
+        } else
+            return false;
+    }
+
+    return true;
+}
+
+float LSM303DLH::getHeading() {
+    return getHeading(0, 1, 0);
+}
+
+float LSM303DLH::getHeading(float x, float y, float z) {
+    Vector base(x, y, z);
+    Vector accel = Vector(ax, ay, az);
+    Vector compass = Vector(mx, my, mz);
+    Vector east = compass * accel;
+    Vector north = accel * east;
+    east /= east.norm();
+    north /= north.norm();
+    //printf("accel = (%.1f, %.1f, %.1f), compass = (%.1f, %.1f, %.1f)\n", accel.x, accel.y, accel.z, compass.x, compass.y, compass.z);
+    //printf("north = (%.1f, %.1f, %.1f), east = (%.1f, %.1f, %.1f)\n", north.x, north.y, north.z, east.x, east.y, east.z);
+    //printf("east.dot(base) = %.1f, north.dot(base) = %.1f\n", east.dot(base), north.dot(base));
+
+    return atan2(east.dot(base), north.dot(base));
+}
+
+void LSM303DLH::attach(INT_TYPE type, char config, char threshold, char duration, void (*handler)(void)) {
+    InterruptIn& interruptIn = (type == INT1) ? int1 : int2;
+    send(SAD_ACC, type == INT1 ? INT1_CFG_A : INT2_CFG_A, config);
+    send(SAD_ACC, type == INT1 ? INT1_THS_A : INT2_THS_A, threshold);
+    send(SAD_ACC, type == INT1 ? INT1_DURATION_A : INT2_DURATION_A, duration);
+    interruptIn.rise(handler);
+}
+
+template<typename T> void LSM303DLH::attach(T *t, INT_TYPE type, char config, char threshold, char duration, void (*handler)(void)) {
+    InterruptIn& interruptIn = (type == INT1) ? int1 : int2;
+    interruptIn.rise(t, handler);
+}
+
+bool LSM303DLH::recv(char sad, char sub, char *buf, int length) {
+    if (length > 1) sub |= 0x80;
+
+    return i2c.write(sad, &sub, 1, true) == 0 && i2c.read(sad, buf, length) == 0;
+}
+
+bool LSM303DLH::send(char sad, char sub, char data) {
+    char buf[] = {sub, data};
+
+    return i2c.write(sad, buf, 2) == 0;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LSM303DLH.h	Thu Oct 06 08:38:06 2011 +0000
@@ -0,0 +1,66 @@
+#ifndef LSM303DLH_H
+#define LSM303DLH_H
+
+#include "mbed.h"
+
+/**/
+enum ACC_ODR {
+    ACC_ODR_50HZ = 0,
+    ACC_ODR_100HZ = 1,
+    ACC_ODR_400HZ = 2,
+    ACC_ODR_1000HZ = 3,
+};
+
+enum ACC_RANGE {
+    ACC_RANGE_2G = 0,
+    ACC_RANGE_4G = 1,
+    ACC_RANGE_8G = 3,
+};
+
+enum MAG_ODR {
+    MAG_ODR_0_75HZ = 0,
+    MAG_ODR_1_5HZ = 1,
+    MAG_ODR_3_0HZ = 2,
+    MAG_ODR_7_5HZ = 3,
+    MAG_ODR_15HZ = 4,
+    MAG_ODR_30HZ = 5,
+    MAG_ODR_75HZ = 6,
+};
+
+enum MAG_RANGE {
+    MAG_RANGE_1_3GAUSS = 1,
+    MAG_RANGE_1_9GAUSS = 2,
+    MAG_RANGE_2_5GAUSS = 3,
+    MAG_RANGE_4_0GAUSS = 4,
+    MAG_RANGE_4_7GAUSS = 5,
+    MAG_RANGE_5_6GAUSS = 6,
+    MAG_RANGE_8_1GAUSS = 7,
+};
+
+enum INT_TYPE {INT1, INT2};
+
+class LSM303DLH {
+public:
+    float ax, ay, az;
+    float mx, my, mz;
+
+    LSM303DLH(PinName sda = p9, PinName scl = p10, PinName int1 = NC, PinName int2 = NC);
+    bool setOutputDataRate(ACC_ODR acc_odr, MAG_ODR mag_odr);
+    bool setMeasurementRange(ACC_RANGE acc_range, MAG_RANGE mag_range);
+    bool read();
+    bool read(float *ax, float *ay, float *az, float *mx, float *my, float *mz);
+    float getHeading();
+    float getHeading(float x, float y, float z);
+    void attach(INT_TYPE type, char config, char threshold, char duration, void (*handler)(void));
+    template<typename T> void attach(T *t, INT_TYPE type, char config, char threshold, char duration, void (*handler)(void));
+
+private:
+    I2C i2c;
+    int acc_range, mag_range;
+    InterruptIn int1, int2;
+
+    bool recv(char sad, char sub, char *buf, int length);
+    bool send(char sad, char sub, char data);
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Vector.h	Thu Oct 06 08:38:06 2011 +0000
@@ -0,0 +1,47 @@
+#ifndef VECTOR_H
+#define VECTOR_H
+
+#include "mbed.h"
+
+class Vector {
+public:
+    float x, y, z;
+    
+    Vector(): x(0), y(0), z(0) {}
+    
+    Vector(float x, float y, float z) : x(x), y(y), z(z) {}
+    
+    Vector(const Vector& v) {
+        x = v.x, y = v.y, z = v.z;
+    }
+
+    Vector product(const Vector& v) {
+        return Vector(y * v.z - z * v.y, z * v.x - x * v.z, x * v.y - y * v.x);
+    }
+
+    float dot(const Vector& v) {
+        return x * v.x + y * v.y + z * v.z;
+    }
+
+    Vector scale(const float c) {
+        return Vector(c * x, c * y, c * z);
+    }
+
+    float norm() {
+        return sqrt(x * x + y * y + z * z);
+    }
+
+    Vector operator*(const Vector& v) {
+        return product(v);
+    }
+
+    Vector operator /=(float c) {
+        return *this = scale(1 / c);
+    }
+
+    Vector operator *=(float c) {
+        return *this = scale(c);
+    }
+};
+
+#endif
\ No newline at end of file