work nice now
Fork of LSM303DLHC by
Diff: LSM303DLHC.cpp
- Revision:
- 6:fcf7e9b8ce21
- Parent:
- 5:5f3ae033c62d
- Child:
- 7:ddd9717cdb71
diff -r 5f3ae033c62d -r fcf7e9b8ce21 LSM303DLHC.cpp --- a/LSM303DLHC.cpp Tue Nov 12 17:25:40 2013 +0000 +++ b/LSM303DLHC.cpp Tue Nov 12 18:30:37 2013 +0000 @@ -109,6 +109,7 @@ bool LSM303DLHC::read(float *ax, float *ay, float *az, float *mx, float *my, float *mz) { char acc[6], mag[6]; + char temp; if (recv(addr_acc, OUT_X_A, acc, 6) && recv(addr_mag, OUT_X_M, mag, 6)) { *ax = *((short*)(acc))/8192.0; //32768/4=8192 @@ -116,9 +117,15 @@ *az = *((short*)(acc+4))/8192.0; //full scale magnetic readings are from -2048 to 2047 //gain is x,y =1100; z = 980 LSB/gauss - *mx = ((((short)mag[0]) << 8) | (0xff&mag[1]))/1100.0; - *my = ((((short)mag[2]) << 8) | (0xff&mag[3]))/1100.0; - *mz = ((((short)mag[4]) << 8) | (0xff&mag[5]))/980.0; + for(int i=0; i<6; i+=2) + { + temp = mag[i]; + mag[i] = mag[i+1]; + mag[i+1] = temp; + } + *mx = *((short*)(mag))/1100.0; + *my = *((short*)(mag+2))/1100.0; + *mz = *((short*)(mag+4))/980.0; return true; }