Diff: LSM303DLHC/LSM303DLHC.cpp

Revision:

23:9be034083282

Child:

38:d0b34cb4d703

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LSM303DLHC/LSM303DLHC.cpp	Mon Aug 14 04:56:52 2017 +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;
+}
+