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