NA
Fork of L3G4200D by
Embed:
(wiki syntax)
Show/hide line numbers
L3G4200D.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 <L3G4200D.h> 00031 #include <math.h> 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 0xD2 00040 00041 // Public Methods ////////////////////////////////////////////////////////////// 00042 00043 // Constructor 00044 L3G4200D::L3G4200D(PinName sda, PinName scl): 00045 _device(sda, scl) 00046 { 00047 _device.frequency(400000); 00048 // Turns on the L3G4200D's gyro and places it in normal mode. 00049 // 0x0F = 0b00001111 00050 // Normal power mode, all axes enabled 00051 writeReg(L3G4200D_CTRL_REG1, 0x0F); 00052 writeReg(L3G4200D_CTRL_REG4, 0x20); // 2000 dps full scale 00053 00054 } 00055 00056 // Writes a gyro register 00057 void L3G4200D::writeReg(byte reg, byte value) 00058 { 00059 data[0] = reg; 00060 data[1] = value; 00061 00062 _device.write(GYR_ADDRESS, data, 2); 00063 } 00064 00065 // Reads a gyro register 00066 byte L3G4200D::readReg(byte reg) 00067 { 00068 byte value = 0; 00069 00070 _device.write(GYR_ADDRESS, ®, 1); 00071 _device.read(GYR_ADDRESS, &value, 1); 00072 00073 return value; 00074 } 00075 00076 // Reads the 3 gyro channels and stores them in vector g 00077 void L3G4200D::read(float g[3]) 00078 { 00079 // assert the MSB of the address to get the gyro 00080 // to do slave-transmit subaddress updating. 00081 data[0] = L3G4200D_OUT_X_L | (1 << 7); 00082 _device.write(GYR_ADDRESS, data, 1); 00083 00084 // Wire.requestFrom(GYR_ADDRESS, 6); 00085 // while (Wire.available() < 6); 00086 00087 _device.read(GYR_ADDRESS, data, 6); 00088 00089 uint8_t xla = data[0]; 00090 uint8_t xha = data[1]; 00091 uint8_t yla = data[2]; 00092 uint8_t yha = data[3]; 00093 uint8_t zla = data[4]; 00094 uint8_t zha = data[5]; 00095 00096 g[0] = (short) (xha << 8 | xla); 00097 g[1] = (short) (yha << 8 | yla); 00098 g[2] = (short) (zha << 8 | zla); 00099 }
Generated on Fri Jul 15 2022 13:18:42 by 1.7.2