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; +} +