FTE-denshi / Mbed 2 deprecated FTE-06

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed FATFileSystem

Fork of FTE-06 by Tetsushi Amano

LSM303DLHC/LSM303DLHC.cpp@38:d0b34cb4d703, 2017-08-21 (annotated)

Committer:
moai26
Date:
Mon Aug 21 02:12:12 2017 +0000
Revision:
38:d0b34cb4d703
Parent:
23:9be034083282 
LSM303????????

Who changed what in which revision?

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