Michael Shimniok / Mbed 2 deprecated DataBus

Code for autonomous rover for Sparkfun AVC. DataBus won 3rd in 2012 and the same code was used on Troubled Child, a 1986 Jeep Grand Wagoneer to win 1st in 2014.

Dependencies:   mbed Watchdog SDFileSystem DigoleSerialDisp

Sensors/LSM303DLM/LSM303DLM.cpp@25:bb5356402687, 2018-11-30 (annotated)

Committer:
shimniok
Date:
Fri Nov 30 16:11:53 2018 +0000
Revision:
25:bb5356402687
Parent:
0:a6a169de725f 
Initial publish of revised version.

Who changed what in which revision?

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