Renan Alves / L3GD20

functions for dealing with the L3GD20 gyro

Fork of L3GD20 by brian claus

L3GD20.cpp@3:010dc7e62988, 2018-10-14 (annotated)

Committer:
renanbmx123
Date:
Sun Oct 14 02:00:17 2018 +0000
Revision:
3:010dc7e62988
Parent:
2:b45dbca259f8 
aa

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bclaus 0:62dfce144cf7 1 /**
bclaus 0:62dfce144cf7 2 * Copyright (c) 2011 Pololu Corporation. For more information, see
bclaus 0:62dfce144cf7 3 *
bclaus 0:62dfce144cf7 4 * http://www.pololu.com/
bclaus 0:62dfce144cf7 5 * http://forum.pololu.com/
bclaus 0:62dfce144cf7 6 *
bclaus 0:62dfce144cf7 7 * Permission is hereby granted, free of charge, to any person
bclaus 0:62dfce144cf7 8 * obtaining a copy of this software and associated documentation
bclaus 0:62dfce144cf7 9 * files (the "Software"), to deal in the Software without
bclaus 0:62dfce144cf7 10 * restriction, including without limitation the rights to use,
bclaus 0:62dfce144cf7 11 * copy, modify, merge, publish, distribute, sublicense, and/or sell
bclaus 0:62dfce144cf7 12 * copies of the Software, and to permit persons to whom the
bclaus 0:62dfce144cf7 13 * Software is furnished to do so, subject to the following
bclaus 0:62dfce144cf7 14 * conditions:
bclaus 0:62dfce144cf7 15 *
bclaus 0:62dfce144cf7 16 * The above copyright notice and this permission notice shall be
bclaus 0:62dfce144cf7 17 * included in all copies or substantial portions of the Software.
bclaus 0:62dfce144cf7 18 *
bclaus 0:62dfce144cf7 19 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
bclaus 0:62dfce144cf7 20 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
bclaus 0:62dfce144cf7 21 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
bclaus 0:62dfce144cf7 22 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
bclaus 0:62dfce144cf7 23 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
bclaus 0:62dfce144cf7 24 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
bclaus 0:62dfce144cf7 25 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
bclaus 0:62dfce144cf7 26 * OTHER DEALINGS IN THE SOFTWARE.
bclaus 0:62dfce144cf7 27 */
bclaus 0:62dfce144cf7 28
bclaus 0:62dfce144cf7 29 #include "mbed.h"
bclaus 0:62dfce144cf7 30 #include "L3GD20.h"
bclaus 0:62dfce144cf7 31
bclaus 0:62dfce144cf7 32
bclaus 0:62dfce144cf7 33 // Defines ////////////////////////////////////////////////////////////////
bclaus 0:62dfce144cf7 34
bclaus 0:62dfce144cf7 35 // The Arduino two-wire interface uses a 7-bit number for the address,
bclaus 0:62dfce144cf7 36 // and sets the last bit correctly based on reads and writes
bclaus 0:62dfce144cf7 37 // mbed I2C libraries take the 7-bit address shifted left 1 bit
bclaus 0:62dfce144cf7 38 // #define GYR_ADDRESS (0xD2 >> 1)
bclaus 0:62dfce144cf7 39 #define GYR_ADDRESS 0xD6
bclaus 0:62dfce144cf7 40
bclaus 0:62dfce144cf7 41
bclaus 0:62dfce144cf7 42 // Public Methods //////////////////////////////////////////////////////////////
bclaus 0:62dfce144cf7 43
bclaus 0:62dfce144cf7 44 // Constructor
bclaus 0:62dfce144cf7 45 L3GD20::L3GD20(PinName sda, PinName scl):
bclaus 0:62dfce144cf7 46 _L3GD20(sda, scl)
bclaus 0:62dfce144cf7 47 {
bclaus 0:62dfce144cf7 48 char reg_v;
bclaus 1:37096bc2cc39 49 _L3GD20.frequency(200000);
bclaus 0:62dfce144cf7 50
bclaus 2:b45dbca259f8 51 reg_v = 0;
bclaus 2:b45dbca259f8 52 write_reg(GYR_ADDRESS,L3GD20_LOW_ODR,reg_v);
bclaus 2:b45dbca259f8 53
bclaus 1:37096bc2cc39 54 // 0x6F
bclaus 1:37096bc2cc39 55 // DR = 01 (200 Hz ODR); BW = 10 (50 Hz bandwidth); PD = 1 (normal mode); Zen = Yen = Xen = 1 (all axes enabled)
bclaus 0:62dfce144cf7 56 reg_v = 0;
bclaus 1:37096bc2cc39 57 reg_v |= 0x6F;
bclaus 0:62dfce144cf7 58 write_reg(GYR_ADDRESS,L3GD20_CTRL_REG1,reg_v);
bclaus 0:62dfce144cf7 59
bclaus 0:62dfce144cf7 60
bclaus 0:62dfce144cf7 61
bclaus 0:62dfce144cf7 62 }
bclaus 0:62dfce144cf7 63
bclaus 0:62dfce144cf7 64
bclaus 0:62dfce144cf7 65
bclaus 0:62dfce144cf7 66 bool L3GD20::read(float *gx, float *gy, float *gz) {
bclaus 0:62dfce144cf7 67 char gyr[6];
bclaus 0:62dfce144cf7 68
bclaus 0:62dfce144cf7 69 if (recv(GYR_ADDRESS, L3GD20_OUT_X_L, gyr, 6)) {
bclaus 0:62dfce144cf7 70 //scale is 8.75 mdps/digit
bclaus 0:62dfce144cf7 71 *gx = float(short(gyr[1] << 8 | gyr[0]))*0.00875;
bclaus 0:62dfce144cf7 72 *gy = float(short(gyr[3] << 8 | gyr[2]))*0.00875;
bclaus 0:62dfce144cf7 73 *gz = float(short(gyr[5] << 8 | gyr[4]))*0.00875;
bclaus 0:62dfce144cf7 74
bclaus 0:62dfce144cf7 75
bclaus 0:62dfce144cf7 76 return true;
bclaus 0:62dfce144cf7 77 }
bclaus 0:62dfce144cf7 78
bclaus 0:62dfce144cf7 79 return false;
bclaus 0:62dfce144cf7 80 }
bclaus 0:62dfce144cf7 81
bclaus 0:62dfce144cf7 82
bclaus 0:62dfce144cf7 83
bclaus 0:62dfce144cf7 84
bclaus 0:62dfce144cf7 85 bool L3GD20::write_reg(int addr_i2c,int addr_reg, char v)
bclaus 0:62dfce144cf7 86 {
bclaus 0:62dfce144cf7 87 char data[2] = {addr_reg, v};
bclaus 0:62dfce144cf7 88 return L3GD20::_L3GD20.write(addr_i2c, data, 2) == 0;
bclaus 0:62dfce144cf7 89 }
bclaus 0:62dfce144cf7 90
bclaus 0:62dfce144cf7 91 bool L3GD20::read_reg(int addr_i2c,int addr_reg, char *v)
bclaus 0:62dfce144cf7 92 {
bclaus 0:62dfce144cf7 93 char data = addr_reg;
bclaus 0:62dfce144cf7 94 bool result = false;
bclaus 0:62dfce144cf7 95
bclaus 0:62dfce144cf7 96 __disable_irq();
bclaus 0:62dfce144cf7 97 if ((_L3GD20.write(addr_i2c, &data, 1) == 0) && (_L3GD20.read(addr_i2c, &data, 1) == 0)){
bclaus 0:62dfce144cf7 98 *v = data;
bclaus 0:62dfce144cf7 99 result = true;
bclaus 0:62dfce144cf7 100 }
bclaus 0:62dfce144cf7 101 __enable_irq();
bclaus 0:62dfce144cf7 102 return result;
bclaus 0:62dfce144cf7 103 }
bclaus 0:62dfce144cf7 104
bclaus 0:62dfce144cf7 105
bclaus 0:62dfce144cf7 106 bool L3GD20::recv(char sad, char sub, char *buf, int length) {
bclaus 0:62dfce144cf7 107 if (length > 1) sub |= 0x80;
bclaus 0:62dfce144cf7 108
bclaus 0:62dfce144cf7 109 return _L3GD20.write(sad, &sub, 1, true) == 0 && _L3GD20.read(sad, buf, length) == 0;
bclaus 0:62dfce144cf7 110 }