Dai Yokota
LSM303DLHC 3D accelerometer, 3D magnetometer and thermometer
Dependents: MAPLEminiA MAPLEminiA
LSM303DLHC is 3D accelerometer and 3D magnetometer module
このチップのライブラリは既にいくつかあるみたいですが、どれも方位取得に特化していて加速度計だけのデータを得ることが面倒（しかもI2Cアドレスが間違ってたりする）なのと内蔵の温度計の値を得ることが出来なかったので、新たに作ってみました。
方位の計算部分は既存のライブラリの物を流用しています。
Diff: LSM303DLHC.cpp
- Revision:
- 0:e5bf52560a0c
--- /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, ®, 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;
+}
+