Haruki Sashida / LSM303D

STM32F303K8T6でLSM303D動かしてみた

Dependents:   GPS_STM32F303K8

LSM303D.cpp@3:50ff4ad71082, 2020-09-08 (annotated)

Committer:
sashida_h
Date:
Tue Sep 08 06:34:37 2020 +0000
Revision:
3:50ff4ad71082
Parent:
2:8cd0c3c11b48 
initial commit

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 const int addr_acc_mag = 0x3A;
bclaus 0:5b930e09bd6e 41
bclaus 0:5b930e09bd6e 42 enum REG_ADDRS {
bclaus 0:5b930e09bd6e 43 /* --- Mag --- */
bclaus 0:5b930e09bd6e 44 OUT_X_M = 0x08,
bclaus 0:5b930e09bd6e 45 OUT_Y_M = 0x0A,
bclaus 0:5b930e09bd6e 46 OUT_Z_M = 0x0C,
bclaus 0:5b930e09bd6e 47 /* --- Acc --- */
bclaus 0:5b930e09bd6e 48 OUT_X_A = 0x28,
bclaus 0:5b930e09bd6e 49 OUT_Y_A = 0x2A,
bclaus 0:5b930e09bd6e 50 OUT_Z_A = 0x2C,
bclaus 0:5b930e09bd6e 51 //
bclaus 1:f93f4bc7a889 52 CTRL0 = 0x1F,
bclaus 1:f93f4bc7a889 53 CTRL1 = 0x20,
bclaus 1:f93f4bc7a889 54 CTRL2 = 0x21,
bclaus 1:f93f4bc7a889 55 CTRL3 = 0x22,
bclaus 1:f93f4bc7a889 56 CTRL4 = 0x23,
bclaus 1:f93f4bc7a889 57 CTRL5 = 0x24,
bclaus 1:f93f4bc7a889 58 CTRL6 = 0x25,
bclaus 1:f93f4bc7a889 59 CTRL7 = 0x26
bclaus 0:5b930e09bd6e 60 };
bclaus 0:5b930e09bd6e 61
bclaus 0:5b930e09bd6e 62 bool LSM303D::write_reg(int addr_i2c,int addr_reg, char v)
bclaus 0:5b930e09bd6e 63 {
bclaus 0:5b930e09bd6e 64 char data[2] = {addr_reg, v};
bclaus 0:5b930e09bd6e 65 return LSM303D::_LSM303.write(addr_i2c, data, 2) == 0;
bclaus 0:5b930e09bd6e 66 }
bclaus 0:5b930e09bd6e 67
bclaus 0:5b930e09bd6e 68 bool LSM303D::read_reg(int addr_i2c,int addr_reg, char *v)
bclaus 0:5b930e09bd6e 69 {
bclaus 0:5b930e09bd6e 70 char data = addr_reg;
bclaus 0:5b930e09bd6e 71 bool result = false;
bclaus 0:5b930e09bd6e 72
bclaus 0:5b930e09bd6e 73 __disable_irq();
bclaus 0:5b930e09bd6e 74 if ((_LSM303.write(addr_i2c, &data, 1) == 0) && (_LSM303.read(addr_i2c, &data, 1) == 0)){
bclaus 0:5b930e09bd6e 75 *v = data;
bclaus 0:5b930e09bd6e 76 result = true;
bclaus 0:5b930e09bd6e 77 }
bclaus 0:5b930e09bd6e 78 __enable_irq();
bclaus 0:5b930e09bd6e 79 return result;
bclaus 0:5b930e09bd6e 80 }
bclaus 0:5b930e09bd6e 81
bclaus 0:5b930e09bd6e 82 LSM303D::LSM303D(PinName sda, PinName scl):
bclaus 0:5b930e09bd6e 83 _LSM303(sda, scl)
bclaus 0:5b930e09bd6e 84 {
bclaus 0:5b930e09bd6e 85 char reg_v;
bclaus 0:5b930e09bd6e 86
bclaus 0:5b930e09bd6e 87 reg_v = 0;
bclaus 0:5b930e09bd6e 88
bclaus 0:5b930e09bd6e 89 //acc
bclaus 0:5b930e09bd6e 90 reg_v |= 0x57; /* 50 Hz X/Y/Z axis enable. */
bclaus 0:5b930e09bd6e 91 write_reg(addr_acc_mag,CTRL1,reg_v);
bclaus 0:5b930e09bd6e 92
bclaus 0:5b930e09bd6e 93 /* -- mag --- */
bclaus 0:5b930e09bd6e 94 reg_v = 0;
sashida_h 3:50ff4ad71082 95 reg_v |= 0x74; //M_RES = 11 (high resolution mode); M_ODR = 100 (50 Hz ODR)
bclaus 0:5b930e09bd6e 96 write_reg(addr_acc_mag,CTRL5,reg_v);
bclaus 0:5b930e09bd6e 97
bclaus 0:5b930e09bd6e 98
bclaus 0:5b930e09bd6e 99
bclaus 0:5b930e09bd6e 100 reg_v = 0;
bclaus 0:5b930e09bd6e 101 reg_v |= 0x00; //continuous mag
bclaus 1:f93f4bc7a889 102 write_reg(addr_acc_mag,CTRL7,reg_v);
sashida_h 3:50ff4ad71082 103
sashida_h 3:50ff4ad71082 104 reg_v = 0;
sashida_h 3:50ff4ad71082 105 reg_v |= 0x00; //+-2gauss
sashida_h 3:50ff4ad71082 106 write_reg(addr_acc_mag,CTRL6,reg_v);
bclaus 0:5b930e09bd6e 107 }
bclaus 0:5b930e09bd6e 108
bclaus 0:5b930e09bd6e 109
sashida_h 3:50ff4ad71082 110 bool LSM303D::read(float *aData,float *mData) {
bclaus 0:5b930e09bd6e 111 char acc[6], mag[6];
bclaus 0:5b930e09bd6e 112
bclaus 0:5b930e09bd6e 113 if (recv(addr_acc_mag, OUT_X_A, acc, 6) && recv(addr_acc_mag, OUT_X_M, mag, 6)) {
sashida_h 3:50ff4ad71082 114 aData[0] = float(short(acc[1] << 8 | acc[0]))*0.061; //32768/4=8192
sashida_h 3:50ff4ad71082 115 aData[1] = float(short(acc[3] << 8 | acc[2]))*0.061;
sashida_h 3:50ff4ad71082 116 aData[2] = float(short(acc[5] << 8 | acc[4]))*0.061;
sashida_h 3:50ff4ad71082 117 //+-2gauss
sashida_h 3:50ff4ad71082 118 mData[0] = float(short(mag[1] << 8 | mag[0]))*0.08;
sashida_h 3:50ff4ad71082 119 mData[1] = float(short(mag[3] << 8 | mag[2]))*0.08;
sashida_h 3:50ff4ad71082 120 mData[2] = float(short(mag[5] << 8 | mag[4]))*0.08;
bclaus 0:5b930e09bd6e 121
bclaus 0:5b930e09bd6e 122 return true;
bclaus 0:5b930e09bd6e 123 }
bclaus 0:5b930e09bd6e 124
bclaus 0:5b930e09bd6e 125 return false;
bclaus 0:5b930e09bd6e 126 }
bclaus 0:5b930e09bd6e 127
bclaus 0:5b930e09bd6e 128
bclaus 0:5b930e09bd6e 129 bool LSM303D::recv(char sad, char sub, char *buf, int length) {
bclaus 0:5b930e09bd6e 130 if (length > 1) sub |= 0x80;
bclaus 0:5b930e09bd6e 131
bclaus 0:5b930e09bd6e 132 return _LSM303.write(sad, &sub, 1, true) == 0 && _LSM303.read(sad, buf, length) == 0;
bclaus 0:5b930e09bd6e 133 }