Dai Yokota / LSM303DLHC

LSM303DLHC 3D accelerometer, 3D magnetometer and thermometer

Dependents:   MAPLEminiA MAPLEminiA

LSM303DLHC is 3D accelerometer and 3D magnetometer module

このチップのライブラリは既にいくつかあるみたいですが、どれも方位取得に特化していて加速度計だけのデータを得ることが面倒（しかもI2Cアドレスが間違ってたりする）なのと内蔵の温度計の値を得ることが出来なかったので、新たに作ってみました。
方位の計算部分は既存のライブラリの物を流用しています。

LSM303DLHC.cpp@0:e5bf52560a0c, 2016-06-12 (annotated)

Committer:
jk1lot
Date:
Sun Jun 12 14:37:01 2016 +0000
Revision:
0:e5bf52560a0c
first published version

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jk1lot 0:e5bf52560a0c 1 #include "LSM303DLHC.h"
jk1lot 0:e5bf52560a0c 2
jk1lot 0:e5bf52560a0c 3 #ifndef M_PI
jk1lot 0:e5bf52560a0c 4 #define M_PI 3.14159265358979323846
jk1lot 0:e5bf52560a0c 5 #endif
jk1lot 0:e5bf52560a0c 6
jk1lot 0:e5bf52560a0c 7 LSM303DLHC::LSM303DLHC(I2C *obj) : i2c(obj)
jk1lot 0:e5bf52560a0c 8 {
jk1lot 0:e5bf52560a0c 9 write(ACCEL_SAD, CTRL_REG1_A, 0x57);
jk1lot 0:e5bf52560a0c 10
jk1lot 0:e5bf52560a0c 11 write(MAGNET_SAD, CRA_REG_M, 0x90);
jk1lot 0:e5bf52560a0c 12 write(MAGNET_SAD, CRB_REG_M, 0x20);
jk1lot 0:e5bf52560a0c 13 write(MAGNET_SAD, MR_REG_M, 0x00);
jk1lot 0:e5bf52560a0c 14 }
jk1lot 0:e5bf52560a0c 15
jk1lot 0:e5bf52560a0c 16 float LSM303DLHC::orientation(void)
jk1lot 0:e5bf52560a0c 17 {
jk1lot 0:e5bf52560a0c 18 /* 加速度センサ未使用版
jk1lot 0:e5bf52560a0c 19 float ans = atan2(float(magneticY()), float(magneticX()));
jk1lot 0:e5bf52560a0c 20 if (ans < 0) ans += 2*M_PI ;
jk1lot 0:e5bf52560a0c 21 if (ans > 2*M_PI) ans -= 2*M_PI;
jk1lot 0:e5bf52560a0c 22 ans = ans * 180/M_PI ;
jk1lot 0:e5bf52560a0c 23 if (ans > 360.0) ans = ans - 360.0 ;
jk1lot 0:e5bf52560a0c 24 return ans ;
jk1lot 0:e5bf52560a0c 25 */
jk1lot 0:e5bf52560a0c 26 //加速度センサ使用版
jk1lot 0:e5bf52560a0c 27 return heading((vector){0,-1,0});
jk1lot 0:e5bf52560a0c 28 }
jk1lot 0:e5bf52560a0c 29
jk1lot 0:e5bf52560a0c 30 float LSM303DLHC::temperature(void)
jk1lot 0:e5bf52560a0c 31 {
jk1lot 0:e5bf52560a0c 32 char data[2];
jk1lot 0:e5bf52560a0c 33 read(MAGNET_SAD, TEMP_OUT_M, data, 2);
jk1lot 0:e5bf52560a0c 34 //マルツにあるサンプルをみたら25を足している
jk1lot 0:e5bf52560a0c 35 //データシートにはそんな記述はないが
jk1lot 0:e5bf52560a0c 36 //確かにそれで、それらしい値になる
jk1lot 0:e5bf52560a0c 37 return ((data[0]<<8)|data[1])/256.0+25;
jk1lot 0:e5bf52560a0c 38 }
jk1lot 0:e5bf52560a0c 39
jk1lot 0:e5bf52560a0c 40 void LSM303DLHC::write(char sad, char reg, char data)
jk1lot 0:e5bf52560a0c 41 {
jk1lot 0:e5bf52560a0c 42 char rw[2];
jk1lot 0:e5bf52560a0c 43 rw[0] = reg;
jk1lot 0:e5bf52560a0c 44 rw[1] = data;
jk1lot 0:e5bf52560a0c 45 i2c->write(sad, rw, 2);
jk1lot 0:e5bf52560a0c 46 }
jk1lot 0:e5bf52560a0c 47
jk1lot 0:e5bf52560a0c 48 void LSM303DLHC::read(char sad, char reg, char *data, int length)
jk1lot 0:e5bf52560a0c 49 {
jk1lot 0:e5bf52560a0c 50 reg |= 0x80; //MSB of register address means auto increment mode
jk1lot 0:e5bf52560a0c 51 i2c->write(sad, &reg, 1);
jk1lot 0:e5bf52560a0c 52 i2c->read(sad, data, length);
jk1lot 0:e5bf52560a0c 53 }
jk1lot 0:e5bf52560a0c 54
jk1lot 0:e5bf52560a0c 55
jk1lot 0:e5bf52560a0c 56 void LSM303DLHC::vector_cross(const vector *a,const vector *b, vector *out)
jk1lot 0:e5bf52560a0c 57 {
jk1lot 0:e5bf52560a0c 58 out->x = a->y*b->z - a->z*b->y;
jk1lot 0:e5bf52560a0c 59 out->y = a->z*b->x - a->x*b->z;
jk1lot 0:e5bf52560a0c 60 out->z = a->x*b->y - a->y*b->x;
jk1lot 0:e5bf52560a0c 61 }
jk1lot 0:e5bf52560a0c 62
jk1lot 0:e5bf52560a0c 63 float LSM303DLHC::vector_dot(const vector *a,const vector *b)
jk1lot 0:e5bf52560a0c 64 {
jk1lot 0:e5bf52560a0c 65 return a->x*b->x+a->y*b->y+a->z*b->z;
jk1lot 0:e5bf52560a0c 66 }
jk1lot 0:e5bf52560a0c 67
jk1lot 0:e5bf52560a0c 68 void LSM303DLHC::vector_normalize(vector *a)
jk1lot 0:e5bf52560a0c 69 {
jk1lot 0:e5bf52560a0c 70 float mag = sqrt(vector_dot(a,a));
jk1lot 0:e5bf52560a0c 71 a->x /= mag;
jk1lot 0:e5bf52560a0c 72 a->y /= mag;
jk1lot 0:e5bf52560a0c 73 a->z /= mag;
jk1lot 0:e5bf52560a0c 74 }
jk1lot 0:e5bf52560a0c 75
jk1lot 0:e5bf52560a0c 76 float LSM303DLHC::heading(vector from)
jk1lot 0:e5bf52560a0c 77 {
jk1lot 0:e5bf52560a0c 78 vector a, m;
jk1lot 0:e5bf52560a0c 79 a.x = accelX();
jk1lot 0:e5bf52560a0c 80 a.y = accelY();
jk1lot 0:e5bf52560a0c 81 a.z = accelZ();
jk1lot 0:e5bf52560a0c 82 m.x = magnetX();
jk1lot 0:e5bf52560a0c 83 m.y = magnetY();
jk1lot 0:e5bf52560a0c 84 m.z = magnetZ();
jk1lot 0:e5bf52560a0c 85
jk1lot 0:e5bf52560a0c 86 ////////////////////////////////////////////////
jk1lot 0:e5bf52560a0c 87 // compute heading
jk1lot 0:e5bf52560a0c 88 ////////////////////////////////////////////////
jk1lot 0:e5bf52560a0c 89
jk1lot 0:e5bf52560a0c 90 vector temp_a = a;
jk1lot 0:e5bf52560a0c 91 // normalize
jk1lot 0:e5bf52560a0c 92 vector_normalize(&temp_a);
jk1lot 0:e5bf52560a0c 93 //vector_normalize(&m);
jk1lot 0:e5bf52560a0c 94
jk1lot 0:e5bf52560a0c 95 // compute E and N
jk1lot 0:e5bf52560a0c 96 vector E;
jk1lot 0:e5bf52560a0c 97 vector N;
jk1lot 0:e5bf52560a0c 98 vector_cross(&m,&temp_a,&E);
jk1lot 0:e5bf52560a0c 99 vector_normalize(&E);
jk1lot 0:e5bf52560a0c 100 vector_cross(&temp_a,&E,&N);
jk1lot 0:e5bf52560a0c 101
jk1lot 0:e5bf52560a0c 102 // compute heading
jk1lot 0:e5bf52560a0c 103 float heading = atan2(vector_dot(&E,&from), vector_dot(&N,&from)) * 180/M_PI;
jk1lot 0:e5bf52560a0c 104 if (heading < 0) heading += 360;
jk1lot 0:e5bf52560a0c 105
jk1lot 0:e5bf52560a0c 106 return heading;
jk1lot 0:e5bf52560a0c 107 }
jk1lot 0:e5bf52560a0c 108