Vojtěch Kolomazník / L3GD20

unterwasserman

Fork of L3GD20 by brian claus

L3GD20.cpp@1:c856665cdb16, 2015-01-26 (annotated)

Committer:
mazejkolo
Date:
Mon Jan 26 19:43:21 2015 +0000
Revision:
1:c856665cdb16
Parent:
0:62dfce144cf7 
BR projekt unterwasserman

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)
mazejkolo 1:c856665cdb16 39
mazejkolo 1:c856665cdb16 40 //#define GYR_ADDRESS 0xD6 //originally
mazejkolo 1:c856665cdb16 41 #define GYR_ADDRESS 0xD0
bclaus 0:62dfce144cf7 42
bclaus 0:62dfce144cf7 43
bclaus 0:62dfce144cf7 44 // Public Methods //////////////////////////////////////////////////////////////
bclaus 0:62dfce144cf7 45
bclaus 0:62dfce144cf7 46 // Constructor
bclaus 0:62dfce144cf7 47 L3GD20::L3GD20(PinName sda, PinName scl):
bclaus 0:62dfce144cf7 48 _L3GD20(sda, scl)
bclaus 0:62dfce144cf7 49 {
bclaus 0:62dfce144cf7 50 char reg_v;
bclaus 0:62dfce144cf7 51 _L3GD20.frequency(100000);
bclaus 0:62dfce144cf7 52
bclaus 0:62dfce144cf7 53 // 0x0F = 0b00001111
bclaus 0:62dfce144cf7 54 // Normal power mode, all axes enabled
bclaus 0:62dfce144cf7 55 reg_v = 0;
bclaus 0:62dfce144cf7 56 reg_v |= 0x0F;
bclaus 0:62dfce144cf7 57 write_reg(GYR_ADDRESS,L3GD20_CTRL_REG1,reg_v);
bclaus 0:62dfce144cf7 58
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 bool L3GD20::read(float *gx, float *gy, float *gz) {
bclaus 0:62dfce144cf7 66 char gyr[6];
bclaus 0:62dfce144cf7 67
bclaus 0:62dfce144cf7 68 if (recv(GYR_ADDRESS, L3GD20_OUT_X_L, gyr, 6)) {
bclaus 0:62dfce144cf7 69 //scale is 8.75 mdps/digit
bclaus 0:62dfce144cf7 70 *gx = float(short(gyr[1] << 8 | gyr[0]))*0.00875;
bclaus 0:62dfce144cf7 71 *gy = float(short(gyr[3] << 8 | gyr[2]))*0.00875;
bclaus 0:62dfce144cf7 72 *gz = float(short(gyr[5] << 8 | gyr[4]))*0.00875;
bclaus 0:62dfce144cf7 73
bclaus 0:62dfce144cf7 74
bclaus 0:62dfce144cf7 75 return true;
bclaus 0:62dfce144cf7 76 }
bclaus 0:62dfce144cf7 77
bclaus 0:62dfce144cf7 78 return false;
bclaus 0:62dfce144cf7 79 }
bclaus 0:62dfce144cf7 80
bclaus 0:62dfce144cf7 81
bclaus 0:62dfce144cf7 82
bclaus 0:62dfce144cf7 83
bclaus 0:62dfce144cf7 84 bool L3GD20::write_reg(int addr_i2c,int addr_reg, char v)
bclaus 0:62dfce144cf7 85 {
bclaus 0:62dfce144cf7 86 char data[2] = {addr_reg, v};
bclaus 0:62dfce144cf7 87 return L3GD20::_L3GD20.write(addr_i2c, data, 2) == 0;
bclaus 0:62dfce144cf7 88 }
bclaus 0:62dfce144cf7 89
bclaus 0:62dfce144cf7 90 bool L3GD20::read_reg(int addr_i2c,int addr_reg, char *v)
bclaus 0:62dfce144cf7 91 {
bclaus 0:62dfce144cf7 92 char data = addr_reg;
bclaus 0:62dfce144cf7 93 bool result = false;
bclaus 0:62dfce144cf7 94
bclaus 0:62dfce144cf7 95 __disable_irq();
bclaus 0:62dfce144cf7 96 if ((_L3GD20.write(addr_i2c, &data, 1) == 0) && (_L3GD20.read(addr_i2c, &data, 1) == 0)){
bclaus 0:62dfce144cf7 97 *v = data;
bclaus 0:62dfce144cf7 98 result = true;
bclaus 0:62dfce144cf7 99 }
bclaus 0:62dfce144cf7 100 __enable_irq();
bclaus 0:62dfce144cf7 101 return result;
bclaus 0:62dfce144cf7 102 }
bclaus 0:62dfce144cf7 103
bclaus 0:62dfce144cf7 104
bclaus 0:62dfce144cf7 105 bool L3GD20::recv(char sad, char sub, char *buf, int length) {
bclaus 0:62dfce144cf7 106 if (length > 1) sub |= 0x80;
bclaus 0:62dfce144cf7 107
bclaus 0:62dfce144cf7 108 return _L3GD20.write(sad, &sub, 1, true) == 0 && _L3GD20.read(sad, buf, length) == 0;
bclaus 0:62dfce144cf7 109 }