DCM Code ported from Arduino for FRDM-KL25Z
Dependents: minimu_data_capture minimu_data_capture
Fork of DCM_AHRS by
L3G4200D.cpp
- Committer:
- ogarai
- Date:
- 2015-01-20
- Revision:
- 2:85214374e094
- Parent:
- 1:3272ece36ce1
File content as of revision 2:85214374e094:
/* mbed L3G4200D Library version 0.1 * Copyright (c) 2012 Ported to MBED and modified by Prediluted (Kris Reynolds) * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ #include <L3G4200D.h> #include <math.h> #include "mbed.h" L3G4200D::L3G4200D( PinName sda = PTE0, PinName scl = PTE1 ) : i2c( sda, scl ) { writeReg( L3G4200D_CTRL_REG1, 0x0f ); } void L3G4200D::writeReg( byte reg, byte value ) { char data[2] = { reg, value }; i2c.write( GYR_ADDRESS, data, 2 ); } char L3G4200D::readReg( byte reg ) { char value[1]; char data[1] = { reg }; i2c.write( GYR_ADDRESS, data, 1 ); i2c.read( GYR_ADDRESS, value, 1 ); return value[0]; } void L3G4200D::read( void ) { char data[1] = { L3G4200D_OUT_X_L | (1<<7) }; char output[6]; i2c.write( GYR_ADDRESS, data, 1 ); i2c.read( GYR_ADDRESS, output, 6 ); g.x = short( output[1] << 8 | output[0] ); g.y = short( output[3] << 8 | output[2] ); g.z = short( output[5] << 8 | output[4] ); } void L3G4200D::vector_cross(const Plane *a,const Plane *b, Plane *out) { out->x = a->y*b->z - a->z*b->y; out->y = a->z*b->x - a->x*b->z; out->z = a->x*b->y - a->y*b->x; } float L3G4200D::vector_dot(const Plane *a,const Plane *b) { return a->x*b->x+a->y*b->y+a->z*b->z; } void L3G4200D::vector_normalize(Plane *a) { float mag = sqrt(vector_dot(a,a)); a->x /= mag; a->y /= mag; a->z /= mag; }