Alexandre Salconi-Denis / L3GD20

My attempt to make a better lib (in development)

Dependents:   Nucleo_i2c_9dof

Fork of L3GD20 by brian claus

L3GD20.cpp@3:17c3c3f59c4d, 2017-08-07 (annotated)

Committer:
salco
Date:
Mon Aug 07 01:25:38 2017 +0000
Revision:
3:17c3c3f59c4d
Parent:
2:b45dbca259f8 
Change the declaration of variable but need checkup because I am not sure everything work.ex: set the register 0x39 and the device dont have this register.

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 // Public Methods //////////////////////////////////////////////////////////////
bclaus 0:62dfce144cf7 42
bclaus 0:62dfce144cf7 43 // Constructor
salco 3:17c3c3f59c4d 44 L3GD20::L3GD20(PinName sda, PinName scl)
salco 3:17c3c3f59c4d 45 {
salco 3:17c3c3f59c4d 46
salco 3:17c3c3f59c4d 47 m_ptr_I2C = new I2C(sda, scl);
salco 3:17c3c3f59c4d 48
salco 3:17c3c3f59c4d 49 init();
salco 3:17c3c3f59c4d 50
salco 3:17c3c3f59c4d 51
salco 3:17c3c3f59c4d 52 }
salco 3:17c3c3f59c4d 53
salco 3:17c3c3f59c4d 54 L3GD20::L3GD20(I2C* pI2C)
salco 3:17c3c3f59c4d 55 {
salco 3:17c3c3f59c4d 56 m_ptr_I2C = pI2C;
salco 3:17c3c3f59c4d 57 init();
salco 3:17c3c3f59c4d 58 }
salco 3:17c3c3f59c4d 59
salco 3:17c3c3f59c4d 60 L3GD20::~L3GD20()
salco 3:17c3c3f59c4d 61 {
salco 3:17c3c3f59c4d 62 delete m_ptr_I2C;
salco 3:17c3c3f59c4d 63 }
salco 3:17c3c3f59c4d 64
salco 3:17c3c3f59c4d 65 void L3GD20::init(void)
bclaus 0:62dfce144cf7 66 {
bclaus 0:62dfce144cf7 67 char reg_v;
salco 3:17c3c3f59c4d 68
salco 3:17c3c3f59c4d 69 m_ptr_I2C->frequency(200000);
bclaus 0:62dfce144cf7 70
salco 3:17c3c3f59c4d 71 reg_v = 0;
salco 3:17c3c3f59c4d 72 if(write_reg(GYR_ADDRESS,L3GD20_LOW_ODR,reg_v) == false)
salco 3:17c3c3f59c4d 73 {
salco 3:17c3c3f59c4d 74 debug("Unable to write in regiter \n");
salco 3:17c3c3f59c4d 75 }
bclaus 2:b45dbca259f8 76
bclaus 1:37096bc2cc39 77 // 0x6F
bclaus 1:37096bc2cc39 78 // DR = 01 (200 Hz ODR); BW = 10 (50 Hz bandwidth); PD = 1 (normal mode); Zen = Yen = Xen = 1 (all axes enabled)
bclaus 0:62dfce144cf7 79 reg_v = 0;
bclaus 1:37096bc2cc39 80 reg_v |= 0x6F;
bclaus 0:62dfce144cf7 81
salco 3:17c3c3f59c4d 82 if(write_reg(GYR_ADDRESS,L3GD20_CTRL_REG1,reg_v) == false)
salco 3:17c3c3f59c4d 83 {
salco 3:17c3c3f59c4d 84 debug("Unable to write in regiter \n");
salco 3:17c3c3f59c4d 85 }
bclaus 0:62dfce144cf7 86 }
salco 3:17c3c3f59c4d 87
salco 3:17c3c3f59c4d 88
bclaus 0:62dfce144cf7 89
bclaus 0:62dfce144cf7 90
bclaus 0:62dfce144cf7 91 bool L3GD20::read(float *gx, float *gy, float *gz) {
bclaus 0:62dfce144cf7 92 char gyr[6];
salco 3:17c3c3f59c4d 93 bool result = false;
salco 3:17c3c3f59c4d 94
salco 3:17c3c3f59c4d 95 if(m_ptr_I2C != NULL)
salco 3:17c3c3f59c4d 96 {
salco 3:17c3c3f59c4d 97 if (recv(GYR_ADDRESS, L3GD20_OUT_X_L, gyr, 6)) {
salco 3:17c3c3f59c4d 98 //scale is 8.75 mdps/digit
salco 3:17c3c3f59c4d 99 *gx = float(short(gyr[1] << 8 | gyr[0]))*0.00875;
salco 3:17c3c3f59c4d 100 *gy = float(short(gyr[3] << 8 | gyr[2]))*0.00875;
salco 3:17c3c3f59c4d 101 *gz = float(short(gyr[5] << 8 | gyr[4]))*0.00875;
salco 3:17c3c3f59c4d 102
salco 3:17c3c3f59c4d 103
salco 3:17c3c3f59c4d 104 result = true;
salco 3:17c3c3f59c4d 105 }
salco 3:17c3c3f59c4d 106 else
salco 3:17c3c3f59c4d 107 {
salco 3:17c3c3f59c4d 108 debug("Unable to receive \n");
salco 3:17c3c3f59c4d 109 }
bclaus 0:62dfce144cf7 110 }
salco 3:17c3c3f59c4d 111 else
salco 3:17c3c3f59c4d 112 {
salco 3:17c3c3f59c4d 113 debug("Pointer null \n");
salco 3:17c3c3f59c4d 114 }
salco 3:17c3c3f59c4d 115 return result;
bclaus 0:62dfce144cf7 116 }
bclaus 0:62dfce144cf7 117
bclaus 0:62dfce144cf7 118
bclaus 0:62dfce144cf7 119
bclaus 0:62dfce144cf7 120
bclaus 0:62dfce144cf7 121 bool L3GD20::write_reg(int addr_i2c,int addr_reg, char v)
bclaus 0:62dfce144cf7 122 {
salco 3:17c3c3f59c4d 123 bool result = false;
bclaus 0:62dfce144cf7 124 char data[2] = {addr_reg, v};
salco 3:17c3c3f59c4d 125 //__disable_irq();
salco 3:17c3c3f59c4d 126 result = m_ptr_I2C->write(addr_i2c, data, 2) == 0;
salco 3:17c3c3f59c4d 127 //__enable_irq();
salco 3:17c3c3f59c4d 128 return result;
bclaus 0:62dfce144cf7 129 }
bclaus 0:62dfce144cf7 130
bclaus 0:62dfce144cf7 131 bool L3GD20::read_reg(int addr_i2c,int addr_reg, char *v)
bclaus 0:62dfce144cf7 132 {
bclaus 0:62dfce144cf7 133 char data = addr_reg;
bclaus 0:62dfce144cf7 134 bool result = false;
bclaus 0:62dfce144cf7 135
salco 3:17c3c3f59c4d 136 //__disable_irq();
salco 3:17c3c3f59c4d 137 if ((m_ptr_I2C->write(addr_i2c, &data, 1) == 0) && (m_ptr_I2C->read(addr_i2c, &data, 1) == 0)){
bclaus 0:62dfce144cf7 138 *v = data;
bclaus 0:62dfce144cf7 139 result = true;
bclaus 0:62dfce144cf7 140 }
salco 3:17c3c3f59c4d 141 //__enable_irq();
bclaus 0:62dfce144cf7 142 return result;
bclaus 0:62dfce144cf7 143 }
bclaus 0:62dfce144cf7 144
bclaus 0:62dfce144cf7 145
bclaus 0:62dfce144cf7 146 bool L3GD20::recv(char sad, char sub, char *buf, int length) {
salco 3:17c3c3f59c4d 147 bool result = false;
salco 3:17c3c3f59c4d 148
bclaus 0:62dfce144cf7 149 if (length > 1) sub |= 0x80;
salco 3:17c3c3f59c4d 150 //__disable_irq();
salco 3:17c3c3f59c4d 151 result = (m_ptr_I2C->write(sad, &sub, 1, true) == 0);
salco 3:17c3c3f59c4d 152 if(result == false) debug("Unable to Write \n");
salco 3:17c3c3f59c4d 153 result = result && (m_ptr_I2C->read(sad, buf, length) == 0);
salco 3:17c3c3f59c4d 154 if(result == false) debug("Unable to Read \n");
salco 3:17c3c3f59c4d 155 //__enable_irq();
salco 3:17c3c3f59c4d 156 return result;
salco 3:17c3c3f59c4d 157 }
salco 3:17c3c3f59c4d 158
salco 3:17c3c3f59c4d 159 bool L3GD20::Convert_to_RadPerSec(float *gx, float *gy, float *gz)
salco 3:17c3c3f59c4d 160 {
salco 3:17c3c3f59c4d 161 bool result = false;
salco 3:17c3c3f59c4d 162 if((gx != NULL) && (gy != NULL) && (gz != NULL) )
salco 3:17c3c3f59c4d 163 {
salco 3:17c3c3f59c4d 164 *gx /= DPS_TO_RPS;
salco 3:17c3c3f59c4d 165 *gy /= DPS_TO_RPS;
salco 3:17c3c3f59c4d 166 *gz /= DPS_TO_RPS;
salco 3:17c3c3f59c4d 167 result = true;
salco 3:17c3c3f59c4d 168 }
salco 3:17c3c3f59c4d 169 return result;
bclaus 0:62dfce144cf7 170 }