functions for dealing with the L3GD20 gyro
Dependents: minimu-9v2 MicroMouse-v1 idd_hw3_ddrew73_fil_clashSword Nucleo_IMU ... more
L3GD20.cpp
00001 /** 00002 * Copyright (c) 2011 Pololu Corporation. For more information, see 00003 * 00004 * http://www.pololu.com/ 00005 * http://forum.pololu.com/ 00006 * 00007 * Permission is hereby granted, free of charge, to any person 00008 * obtaining a copy of this software and associated documentation 00009 * files (the "Software"), to deal in the Software without 00010 * restriction, including without limitation the rights to use, 00011 * copy, modify, merge, publish, distribute, sublicense, and/or sell 00012 * copies of the Software, and to permit persons to whom the 00013 * Software is furnished to do so, subject to the following 00014 * conditions: 00015 * 00016 * The above copyright notice and this permission notice shall be 00017 * included in all copies or substantial portions of the Software. 00018 * 00019 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 00020 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES 00021 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 00022 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 00023 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 00024 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 00025 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR 00026 * OTHER DEALINGS IN THE SOFTWARE. 00027 */ 00028 00029 #include "mbed.h" 00030 #include "L3GD20.h" 00031 00032 00033 // Defines //////////////////////////////////////////////////////////////// 00034 00035 // The Arduino two-wire interface uses a 7-bit number for the address, 00036 // and sets the last bit correctly based on reads and writes 00037 // mbed I2C libraries take the 7-bit address shifted left 1 bit 00038 // #define GYR_ADDRESS (0xD2 >> 1) 00039 #define GYR_ADDRESS 0xD6 00040 00041 00042 // Public Methods ////////////////////////////////////////////////////////////// 00043 00044 // Constructor 00045 L3GD20::L3GD20(PinName sda, PinName scl): 00046 _L3GD20(sda, scl) 00047 { 00048 char reg_v; 00049 _L3GD20.frequency(200000); 00050 00051 reg_v = 0; 00052 write_reg(GYR_ADDRESS,L3GD20_LOW_ODR,reg_v); 00053 00054 // 0x6F 00055 // DR = 01 (200 Hz ODR); BW = 10 (50 Hz bandwidth); PD = 1 (normal mode); Zen = Yen = Xen = 1 (all axes enabled) 00056 reg_v = 0; 00057 reg_v |= 0x6F; 00058 write_reg(GYR_ADDRESS,L3GD20_CTRL_REG1,reg_v); 00059 00060 00061 00062 } 00063 00064 00065 00066 bool L3GD20::read(float *gx, float *gy, float *gz) { 00067 char gyr[6]; 00068 00069 if (recv(GYR_ADDRESS, L3GD20_OUT_X_L, gyr, 6)) { 00070 //scale is 8.75 mdps/digit 00071 *gx = float(short(gyr[1] << 8 | gyr[0]))*0.00875; 00072 *gy = float(short(gyr[3] << 8 | gyr[2]))*0.00875; 00073 *gz = float(short(gyr[5] << 8 | gyr[4]))*0.00875; 00074 00075 00076 return true; 00077 } 00078 00079 return false; 00080 } 00081 00082 00083 00084 00085 bool L3GD20::write_reg(int addr_i2c,int addr_reg, char v) 00086 { 00087 char data[2] = {addr_reg, v}; 00088 return L3GD20::_L3GD20.write(addr_i2c, data, 2) == 0; 00089 } 00090 00091 bool L3GD20::read_reg(int addr_i2c,int addr_reg, char *v) 00092 { 00093 char data = addr_reg; 00094 bool result = false; 00095 00096 __disable_irq(); 00097 if ((_L3GD20.write(addr_i2c, &data, 1) == 0) && (_L3GD20.read(addr_i2c, &data, 1) == 0)){ 00098 *v = data; 00099 result = true; 00100 } 00101 __enable_irq(); 00102 return result; 00103 } 00104 00105 00106 bool L3GD20::recv(char sad, char sub, char *buf, int length) { 00107 if (length > 1) sub |= 0x80; 00108 00109 return _L3GD20.write(sad, &sub, 1, true) == 0 && _L3GD20.read(sad, buf, length) == 0; 00110 }
Generated on Tue Jul 12 2022 18:31:09 by 1.7.2