Adafruit 9DOF - LSM303DLHC
Fork of LSM303DLHC by
LSM303DLHC.cpp@0:ffed7ef0b248, 2016-04-09 (annotated)
- Committer:
- julioefajardo
- Date:
- Sat Apr 09 04:26:50 2016 +0000
- Revision:
- 0:ffed7ef0b248
- Child:
- 1:cd027bdb4892
Adafruit 9DOF - LSM303DLHC Driver
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
julioefajardo | 0:ffed7ef0b248 | 1 | #include "mbed.h" |
julioefajardo | 0:ffed7ef0b248 | 2 | #include "LSM303DLHC.h" |
julioefajardo | 0:ffed7ef0b248 | 3 | #include "stdio.h" |
julioefajardo | 0:ffed7ef0b248 | 4 | |
julioefajardo | 0:ffed7ef0b248 | 5 | #define MAG_ADDRESS 0x3C |
julioefajardo | 0:ffed7ef0b248 | 6 | #define ACC_ADDRESS 0x32 |
julioefajardo | 0:ffed7ef0b248 | 7 | |
julioefajardo | 0:ffed7ef0b248 | 8 | |
julioefajardo | 0:ffed7ef0b248 | 9 | LSM303DLHC::LSM303DLHC(PinName sda, PinName scl): _device(sda, scl) |
julioefajardo | 0:ffed7ef0b248 | 10 | { |
julioefajardo | 0:ffed7ef0b248 | 11 | _device.frequency(400000); |
julioefajardo | 0:ffed7ef0b248 | 12 | // init(); |
julioefajardo | 0:ffed7ef0b248 | 13 | } |
julioefajardo | 0:ffed7ef0b248 | 14 | |
julioefajardo | 0:ffed7ef0b248 | 15 | void LSM303DLHC::init() |
julioefajardo | 0:ffed7ef0b248 | 16 | { |
julioefajardo | 0:ffed7ef0b248 | 17 | // init mag |
julioefajardo | 0:ffed7ef0b248 | 18 | // continuous conversion mode |
julioefajardo | 0:ffed7ef0b248 | 19 | _data[0] = MR_REG_M; |
julioefajardo | 0:ffed7ef0b248 | 20 | _data[1] = 0x00; |
julioefajardo | 0:ffed7ef0b248 | 21 | _device.write(MAG_ADDRESS, _data, 2); |
julioefajardo | 0:ffed7ef0b248 | 22 | // data rate 75hz |
julioefajardo | 0:ffed7ef0b248 | 23 | _data[0] = CRA_REG_M; |
julioefajardo | 0:ffed7ef0b248 | 24 | _data[1] = 0x18; // 0b00011000 |
julioefajardo | 0:ffed7ef0b248 | 25 | _device.write(MAG_ADDRESS, _data, 2); |
julioefajardo | 0:ffed7ef0b248 | 26 | // init acc |
julioefajardo | 0:ffed7ef0b248 | 27 | // data rate 100hz |
julioefajardo | 0:ffed7ef0b248 | 28 | _data[0] = CTRL_REG1_A; |
julioefajardo | 0:ffed7ef0b248 | 29 | _data[1] = 0x2F; // 0b00101111 |
julioefajardo | 0:ffed7ef0b248 | 30 | _device.write(ACC_ADDRESS, _data, 2); |
julioefajardo | 0:ffed7ef0b248 | 31 | } |
julioefajardo | 0:ffed7ef0b248 | 32 | |
julioefajardo | 0:ffed7ef0b248 | 33 | void LSM303DLHC::read(int a[3], int m[3]) |
julioefajardo | 0:ffed7ef0b248 | 34 | { |
julioefajardo | 0:ffed7ef0b248 | 35 | readAcc(a); |
julioefajardo | 0:ffed7ef0b248 | 36 | readMag(m); |
julioefajardo | 0:ffed7ef0b248 | 37 | } |
julioefajardo | 0:ffed7ef0b248 | 38 | |
julioefajardo | 0:ffed7ef0b248 | 39 | void LSM303DLHC::readAcc(int a[3]) |
julioefajardo | 0:ffed7ef0b248 | 40 | { |
julioefajardo | 0:ffed7ef0b248 | 41 | _data[0] = OUT_X_L_A | (1<<7); |
julioefajardo | 0:ffed7ef0b248 | 42 | _device.write(ACC_ADDRESS, _data, 1); |
julioefajardo | 0:ffed7ef0b248 | 43 | _device.read(ACC_ADDRESS, _data, 6); |
julioefajardo | 0:ffed7ef0b248 | 44 | |
julioefajardo | 0:ffed7ef0b248 | 45 | // 12-bit values |
julioefajardo | 0:ffed7ef0b248 | 46 | a[0] = (short)(_data[1]<<8 | _data[0]) >> 4; |
julioefajardo | 0:ffed7ef0b248 | 47 | a[1] = (short)(_data[3]<<8 | _data[2]) >> 4; |
julioefajardo | 0:ffed7ef0b248 | 48 | a[2] = (short)(_data[5]<<8 | _data[4]) >> 4; |
julioefajardo | 0:ffed7ef0b248 | 49 | } |
julioefajardo | 0:ffed7ef0b248 | 50 | |
julioefajardo | 0:ffed7ef0b248 | 51 | void LSM303DLHC::readMag(int m[3]) |
julioefajardo | 0:ffed7ef0b248 | 52 | { |
julioefajardo | 0:ffed7ef0b248 | 53 | _data[0] = OUT_X_H_M; |
julioefajardo | 0:ffed7ef0b248 | 54 | _device.write(MAG_ADDRESS, _data, 1); |
julioefajardo | 0:ffed7ef0b248 | 55 | _device.read(MAG_ADDRESS, _data, 6); |
julioefajardo | 0:ffed7ef0b248 | 56 | |
julioefajardo | 0:ffed7ef0b248 | 57 | m[0] = (short) (_data[0]<<8 | _data[1]); // X |
julioefajardo | 0:ffed7ef0b248 | 58 | m[1] = (short) (_data[4]<<8 | _data[5]); // Y |
julioefajardo | 0:ffed7ef0b248 | 59 | m[2] = (short) (_data[2]<<8 | _data[3]); // Z |
julioefajardo | 0:ffed7ef0b248 | 60 | } |
julioefajardo | 0:ffed7ef0b248 | 61 | |
julioefajardo | 0:ffed7ef0b248 | 62 | void LSM303DLHC::setScale(float x, float y, float z) |
julioefajardo | 0:ffed7ef0b248 | 63 | { |
julioefajardo | 0:ffed7ef0b248 | 64 | scale[0] = x; |
julioefajardo | 0:ffed7ef0b248 | 65 | scale[1] = y; |
julioefajardo | 0:ffed7ef0b248 | 66 | scale[2] = z; |
julioefajardo | 0:ffed7ef0b248 | 67 | } |
julioefajardo | 0:ffed7ef0b248 | 68 | |
julioefajardo | 0:ffed7ef0b248 | 69 | void LSM303DLHC::setOffset(float x, float y, float z) |
julioefajardo | 0:ffed7ef0b248 | 70 | { |
julioefajardo | 0:ffed7ef0b248 | 71 | offset[0] = x; |
julioefajardo | 0:ffed7ef0b248 | 72 | offset[1] = y; |
julioefajardo | 0:ffed7ef0b248 | 73 | offset[2] = z; |
julioefajardo | 0:ffed7ef0b248 | 74 | } |