Revision 0:e5bf52560a0c, committed 2016-06-12

Comitter:

jk1lot

Date:

Sun Jun 12 14:37:01 2016 +0000

Commit message:

first published version

Changed in this revision

diff -r 000000000000 -r e5bf52560a0c LSM303DLHC.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LSM303DLHC.cpp	Sun Jun 12 14:37:01 2016 +0000
@@ -0,0 +1,108 @@
+#include "LSM303DLHC.h"
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+LSM303DLHC::LSM303DLHC(I2C *obj) : i2c(obj)
+{
+    write(ACCEL_SAD, CTRL_REG1_A, 0x57);
+
+    write(MAGNET_SAD, CRA_REG_M, 0x90);
+    write(MAGNET_SAD, CRB_REG_M, 0x20);
+    write(MAGNET_SAD, MR_REG_M, 0x00);
+}
+
+float LSM303DLHC::orientation(void)
+{
+        /* 加速度センサ未使用版
+        float ans = atan2(float(magneticY()), float(magneticX()));
+        if (ans < 0) ans += 2*M_PI ;
+        if (ans > 2*M_PI) ans -= 2*M_PI;
+        ans = ans * 180/M_PI ;
+        if (ans > 360.0) ans = ans - 360.0 ;
+        return ans ;
+        */
+        //加速度センサ使用版
+        return heading((vector){0,-1,0});
+}
+
+float LSM303DLHC::temperature(void)
+{
+    char data[2];
+    read(MAGNET_SAD, TEMP_OUT_M, data, 2);
+    //マルツにあるサンプルをみたら25を足している
+    //データシートにはそんな記述はないが
+    //確かにそれで、それらしい値になる
+    return ((data[0]<<8)|data[1])/256.0+25;
+}
+
+void LSM303DLHC::write(char sad, char reg, char data)
+{
+    char rw[2];
+    rw[0] = reg;
+    rw[1] = data;
+    i2c->write(sad, rw, 2);
+}
+
+void LSM303DLHC::read(char sad, char reg, char *data, int length)
+{
+    reg |= 0x80; //MSB of register address means auto increment mode
+    i2c->write(sad, &reg, 1);
+    i2c->read(sad, data, length);
+}
+
+
+void LSM303DLHC::vector_cross(const vector *a,const vector *b, vector *out)
+{
+  out->x = a->y*b->z - a->z*b->y;
+  out->y = a->z*b->x - a->x*b->z;
+  out->z = a->x*b->y - a->y*b->x;
+}
+
+float LSM303DLHC::vector_dot(const vector *a,const vector *b)
+{
+  return a->x*b->x+a->y*b->y+a->z*b->z;
+}
+
+void LSM303DLHC::vector_normalize(vector *a)
+{
+  float mag = sqrt(vector_dot(a,a));
+  a->x /= mag;
+  a->y /= mag;
+  a->z /= mag;
+}
+
+float LSM303DLHC::heading(vector from)
+{
+    vector a, m;
+    a.x = accelX();
+    a.y = accelY();
+    a.z = accelZ();
+    m.x = magnetX();
+    m.y = magnetY();
+    m.z = magnetZ();
+    
+    ////////////////////////////////////////////////
+    // compute heading       
+    ////////////////////////////////////////////////
+
+    vector temp_a = a;
+    // normalize
+    vector_normalize(&temp_a);
+    //vector_normalize(&m);
+
+    // compute E and N
+    vector E;
+    vector N;
+    vector_cross(&m,&temp_a,&E);
+    vector_normalize(&E);
+    vector_cross(&temp_a,&E,&N);
+    
+    // compute heading
+    float heading = atan2(vector_dot(&E,&from), vector_dot(&N,&from)) * 180/M_PI;
+    if (heading < 0) heading += 360;
+    
+    return heading;
+}
+

diff -r 000000000000 -r e5bf52560a0c LSM303DLHC.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LSM303DLHC.h	Sun Jun 12 14:37:01 2016 +0000
@@ -0,0 +1,95 @@
+#ifndef LSM303DLHC_H
+#define LSM303DLHC_H
+
+#include "mbed.h"
+
+//! LSM303DLHC 3D accelerometer and 3D magnetometer
+class LSM303DLHC {
+    typedef struct vector {float x, y, z;} vector;
+    static void vector_cross(const vector *a, const vector *b, vector *out);
+    static float vector_dot(const vector *a,const vector *b);
+    static void vector_normalize(vector *a);
+public:
+    //! @param obj pointer to I2C object
+    LSM303DLHC(I2C *obj);
+    //! measure accelerometer value
+    void getAccel(void) {read(ACCEL_SAD, OUT_A, _accel, 6);}
+    //! extract X-axis value from mesured accelerometer
+    int16_t accelX(void) {return (_accel[1]<<8)|_accel[0];}
+    //! extract Y-axis value from mesured accelerometer
+    int16_t accelY(void) {return (_accel[3]<<8)|_accel[2];}
+    //! extract Z-axis value from mesured accelerometer
+    int16_t accelZ(void) {return (_accel[5]<<8)|_accel[4];}
+
+    //! mesure magnetometer value
+    void getMagnet(void) {read(MAGNET_SAD, OUT_M, _magnet, 6);}
+    //accelと magnetでは Highと Lowの順序が逆になってる変な仕様
+    //しかもMagnetは X,Z,Y の順番
+    //! extract X-axis value from mesured magnetometer
+    int16_t magnetX(void) {return (_magnet[0]<<8)|_magnet[1];}
+    //! extract Z-axis value from mesured magnetometer
+    int16_t magnetZ(void) {return (_magnet[2]<<8)|_magnet[3];}
+    //! extract Y-axis value from mesured magnetometer
+    int16_t magnetY(void) {return (_magnet[4]<<8)|_magnet[5];}
+
+    float heading(vector from);
+    //! @return orientation(direction) value, expressed in degrees(0~360)
+    float orientation(void);
+    //! @return Temperature value, expressed in degrees Celsius
+    float temperature(void);
+protected:
+    //! write 1byte
+    //! @param sad I2C address
+    //! @param reg register address
+    //! @param data data
+    void write(char sad, char reg, char data);
+    //! read data
+    //! @param sad I2C address
+    //! @param reg register address start from here and auto incriment
+    //! @param data pointer to data storage
+    //! @param length how many bytes to read
+    void read(char sad, char reg, char *data, int length=1);
+
+    I2C *i2c;
+    static const int8_t ACCEL_SAD = 0x32;
+    static const int8_t MAGNET_SAD = 0x3C;
+    enum accel_regs {
+        CTRL_REG1_A=0x20,
+        CTRL_REG2_A=0x21,
+        CTRL_REG3_A=0x22,
+        CTRL_REG4_A=0x23,
+        CTRL_REG5_A=0x24,
+        CTRL_REG6_A=0x25,
+        REFERENCE_A=0x26,
+        STATUS_REG_A=0x27,
+        OUT_A=0x28, //6bytes
+        FIFO_CTRL_REG_A=0x2E,
+        FIFO_SRC_REG_A=0x2F,
+        INT1_A=0x30, //4bytes
+        INT2_A=0x34, //4bytes
+        CLICK_A=0x38, //3bytes
+        TIME_LIMIT_A=0x3B,
+        TIME_LATENCY_A=0x3C,
+        TIME_WINDOW_A=0x3D
+    };
+    static const int INT_CFG=0;
+    static const int INT_SRC=1;
+    static const int INT_THS=2;
+    static const int INT_DURATION=3;
+    enum magnet_regs {
+        CRA_REG_M=0x00,
+        CRB_REG_M=0x01,
+        MR_REG_M=0x02,
+        OUT_M=0x03, //6bytes
+        SR_REG_M=0x09,
+        IRA_REG_M=0x0A,
+        IRB_REG_M=0x0B,
+        IRC_REG_M=0x0C,
+        TEMP_OUT_M=0x31 //2bytes
+    };
+private:
+    char _accel[6];
+    char _magnet[6];
+};
+
+#endif
\ No newline at end of file