brian claus / LSM303D

Port of LSM303DLHC with modified registers to work with LSM303D

Dependents:   Altimu10v4

LSM303D.cpp@2:8cd0c3c11b48, 2016-02-08 (annotated)

Committer:
bclaus
Date:
Mon Feb 08 17:57:22 2016 +0000
Revision:
2:8cd0c3c11b48
Parent:
1:f93f4bc7a889 
update;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bclaus 0:5b930e09bd6e 1 /** LSM303D Interface Library
bclaus 0:5b930e09bd6e 2 *
bclaus 0:5b930e09bd6e 3 * base on code by Michael Shimniok http://bot-thoughts.com
bclaus 0:5b930e09bd6e 4 *
bclaus 0:5b930e09bd6e 5 * and on test program by @tosihisa and
bclaus 0:5b930e09bd6e 6 *
bclaus 0:5b930e09bd6e 7 * and on Pololu sample library for LSM303D breakout by ryantm:
bclaus 0:5b930e09bd6e 8 *
bclaus 0:5b930e09bd6e 9 * ported by Brian Claus
bclaus 0:5b930e09bd6e 10 *
bclaus 0:5b930e09bd6e 11 * Copyright (c) 2011 Pololu Corporation. For more information, see
bclaus 0:5b930e09bd6e 12 *
bclaus 0:5b930e09bd6e 13 * http://www.pololu.com/
bclaus 0:5b930e09bd6e 14 * http://forum.pololu.com/
bclaus 0:5b930e09bd6e 15 *
bclaus 0:5b930e09bd6e 16 * Permission is hereby granted, free of charge, to any person
bclaus 0:5b930e09bd6e 17 * obtaining a copy of this software and associated documentation
bclaus 0:5b930e09bd6e 18 * files (the "Software"), to deal in the Software without
bclaus 0:5b930e09bd6e 19 * restriction, including without limitation the rights to use,
bclaus 0:5b930e09bd6e 20 * copy, modify, merge, publish, distribute, sublicense, and/or sell
bclaus 0:5b930e09bd6e 21 * copies of the Software, and to permit persons to whom the
bclaus 0:5b930e09bd6e 22 * Software is furnished to do so, subject to the following
bclaus 0:5b930e09bd6e 23 * conditions:
bclaus 0:5b930e09bd6e 24 *
bclaus 0:5b930e09bd6e 25 * The above copyright notice and this permission notice shall be
bclaus 0:5b930e09bd6e 26 * included in all copies or substantial portions of the Software.
bclaus 0:5b930e09bd6e 27 *
bclaus 0:5b930e09bd6e 28 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
bclaus 0:5b930e09bd6e 29 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
bclaus 0:5b930e09bd6e 30 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
bclaus 0:5b930e09bd6e 31 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
bclaus 0:5b930e09bd6e 32 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
bclaus 0:5b930e09bd6e 33 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
bclaus 0:5b930e09bd6e 34 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
bclaus 0:5b930e09bd6e 35 * OTHER DEALINGS IN THE SOFTWARE.
bclaus 0:5b930e09bd6e 36 */
bclaus 0:5b930e09bd6e 37 #include "mbed.h"
bclaus 0:5b930e09bd6e 38 #include "LSM303D.h"
bclaus 0:5b930e09bd6e 39
bclaus 0:5b930e09bd6e 40
bclaus 0:5b930e09bd6e 41 const int addr_acc_mag = 0x3A;
bclaus 0:5b930e09bd6e 42
bclaus 0:5b930e09bd6e 43 enum REG_ADDRS {
bclaus 0:5b930e09bd6e 44 /* --- Mag --- */
bclaus 0:5b930e09bd6e 45 OUT_X_M = 0x08,
bclaus 0:5b930e09bd6e 46 OUT_Y_M = 0x0A,
bclaus 0:5b930e09bd6e 47 OUT_Z_M = 0x0C,
bclaus 0:5b930e09bd6e 48 /* --- Acc --- */
bclaus 0:5b930e09bd6e 49 OUT_X_A = 0x28,
bclaus 0:5b930e09bd6e 50 OUT_Y_A = 0x2A,
bclaus 0:5b930e09bd6e 51 OUT_Z_A = 0x2C,
bclaus 0:5b930e09bd6e 52 //
bclaus 1:f93f4bc7a889 53 CTRL0 = 0x1F,
bclaus 1:f93f4bc7a889 54 CTRL1 = 0x20,
bclaus 1:f93f4bc7a889 55 CTRL2 = 0x21,
bclaus 1:f93f4bc7a889 56 CTRL3 = 0x22,
bclaus 1:f93f4bc7a889 57 CTRL4 = 0x23,
bclaus 1:f93f4bc7a889 58 CTRL5 = 0x24,
bclaus 1:f93f4bc7a889 59 CTRL6 = 0x25,
bclaus 1:f93f4bc7a889 60 CTRL7 = 0x26
bclaus 0:5b930e09bd6e 61 };
bclaus 0:5b930e09bd6e 62
bclaus 0:5b930e09bd6e 63 bool LSM303D::write_reg(int addr_i2c,int addr_reg, char v)
bclaus 0:5b930e09bd6e 64 {
bclaus 0:5b930e09bd6e 65 char data[2] = {addr_reg, v};
bclaus 0:5b930e09bd6e 66 return LSM303D::_LSM303.write(addr_i2c, data, 2) == 0;
bclaus 0:5b930e09bd6e 67 }
bclaus 0:5b930e09bd6e 68
bclaus 0:5b930e09bd6e 69 bool LSM303D::read_reg(int addr_i2c,int addr_reg, char *v)
bclaus 0:5b930e09bd6e 70 {
bclaus 0:5b930e09bd6e 71 char data = addr_reg;
bclaus 0:5b930e09bd6e 72 bool result = false;
bclaus 0:5b930e09bd6e 73
bclaus 0:5b930e09bd6e 74 __disable_irq();
bclaus 0:5b930e09bd6e 75 if ((_LSM303.write(addr_i2c, &data, 1) == 0) && (_LSM303.read(addr_i2c, &data, 1) == 0)){
bclaus 0:5b930e09bd6e 76 *v = data;
bclaus 0:5b930e09bd6e 77 result = true;
bclaus 0:5b930e09bd6e 78 }
bclaus 0:5b930e09bd6e 79 __enable_irq();
bclaus 0:5b930e09bd6e 80 return result;
bclaus 0:5b930e09bd6e 81 }
bclaus 0:5b930e09bd6e 82
bclaus 0:5b930e09bd6e 83 LSM303D::LSM303D(PinName sda, PinName scl):
bclaus 0:5b930e09bd6e 84 _LSM303(sda, scl)
bclaus 0:5b930e09bd6e 85 {
bclaus 0:5b930e09bd6e 86 char reg_v;
bclaus 0:5b930e09bd6e 87 _LSM303.frequency(200000);
bclaus 0:5b930e09bd6e 88
bclaus 0:5b930e09bd6e 89 reg_v = 0;
bclaus 0:5b930e09bd6e 90
bclaus 0:5b930e09bd6e 91 //acc
bclaus 0:5b930e09bd6e 92 reg_v |= 0x57; /* 50 Hz X/Y/Z axis enable. */
bclaus 0:5b930e09bd6e 93 write_reg(addr_acc_mag,CTRL1,reg_v);
bclaus 0:5b930e09bd6e 94
bclaus 0:5b930e09bd6e 95 /* -- mag --- */
bclaus 0:5b930e09bd6e 96 reg_v = 0;
bclaus 0:5b930e09bd6e 97 reg_v |= 0x78; //M_RES = 11 (high resolution mode); M_ODR = 100 (50 Hz ODR)
bclaus 0:5b930e09bd6e 98 write_reg(addr_acc_mag,CTRL5,reg_v);
bclaus 0:5b930e09bd6e 99
bclaus 0:5b930e09bd6e 100
bclaus 0:5b930e09bd6e 101
bclaus 0:5b930e09bd6e 102 reg_v = 0;
bclaus 0:5b930e09bd6e 103 reg_v |= 0x00; //continuous mag
bclaus 1:f93f4bc7a889 104 write_reg(addr_acc_mag,CTRL7,reg_v);
bclaus 0:5b930e09bd6e 105 }
bclaus 0:5b930e09bd6e 106
bclaus 0:5b930e09bd6e 107
bclaus 0:5b930e09bd6e 108 bool LSM303D::read(float *ax, float *ay, float *az, float *mx, float *my, float *mz) {
bclaus 0:5b930e09bd6e 109 char acc[6], mag[6];
bclaus 0:5b930e09bd6e 110
bclaus 0:5b930e09bd6e 111 if (recv(addr_acc_mag, OUT_X_A, acc, 6) && recv(addr_acc_mag, OUT_X_M, mag, 6)) {
bclaus 2:8cd0c3c11b48 112 *ax = float(short(acc[1] << 8 | acc[0]))*0.061; //32768/4=8192
bclaus 2:8cd0c3c11b48 113 *ay = float(short(acc[3] << 8 | acc[2]))*0.061;
bclaus 2:8cd0c3c11b48 114 *az = float(short(acc[5] << 8 | acc[4]))*0.061;
bclaus 2:8cd0c3c11b48 115 //+-4gauss
bclaus 2:8cd0c3c11b48 116 *mx = float(short(mag[0] << 8 | mag[1]))*0.16;
bclaus 2:8cd0c3c11b48 117 *mz = float(short(mag[2] << 8 | mag[3]))*0.16;
bclaus 2:8cd0c3c11b48 118 *my = float(short(mag[4] << 8 | mag[5]))*0.16;
bclaus 0:5b930e09bd6e 119
bclaus 0:5b930e09bd6e 120 return true;
bclaus 0:5b930e09bd6e 121 }
bclaus 0:5b930e09bd6e 122
bclaus 0:5b930e09bd6e 123 return false;
bclaus 0:5b930e09bd6e 124 }
bclaus 0:5b930e09bd6e 125
bclaus 0:5b930e09bd6e 126
bclaus 0:5b930e09bd6e 127 bool LSM303D::recv(char sad, char sub, char *buf, int length) {
bclaus 0:5b930e09bd6e 128 if (length > 1) sub |= 0x80;
bclaus 0:5b930e09bd6e 129
bclaus 0:5b930e09bd6e 130 return _LSM303.write(sad, &sub, 1, true) == 0 && _LSM303.read(sad, buf, length) == 0;
bclaus 0:5b930e09bd6e 131 }