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?
User | Revision | Line number | New 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, ®, 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 |