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