A Team / DCM_AHRS

DCM Code ported from Arduino for FRDM-KL25Z

Dependents:   minimu_data_capture minimu_data_capture

Fork of DCM_AHRS by Kris Reynolds

L3G4200D.cpp@2:85214374e094, 2015-01-20 (annotated)

Committer:
ogarai
Date:
Tue Jan 20 02:04:07 2015 +0000
Revision:
2:85214374e094
Parent:
1:3272ece36ce1 
First Commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
krmreynolds 0:dc35364e2291 1 /* mbed L3G4200D Library version 0.1
krmreynolds 1:3272ece36ce1 2 * Copyright (c) 2012 Ported to MBED and modified by Prediluted (Kris Reynolds)
krmreynolds 0:dc35364e2291 3 *
krmreynolds 0:dc35364e2291 4 * Permission is hereby granted, free of charge, to any person obtaining a copy
krmreynolds 0:dc35364e2291 5 * of this software and associated documentation files (the "Software"), to deal
krmreynolds 0:dc35364e2291 6 * in the Software without restriction, including without limitation the rights
krmreynolds 0:dc35364e2291 7 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
krmreynolds 0:dc35364e2291 8 * copies of the Software, and to permit persons to whom the Software is
krmreynolds 0:dc35364e2291 9 * furnished to do so, subject to the following conditions:
krmreynolds 0:dc35364e2291 10 *
krmreynolds 0:dc35364e2291 11 * The above copyright notice and this permission notice shall be included in
krmreynolds 0:dc35364e2291 12 * all copies or substantial portions of the Software.
krmreynolds 0:dc35364e2291 13 *
krmreynolds 0:dc35364e2291 14 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
krmreynolds 0:dc35364e2291 15 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
krmreynolds 0:dc35364e2291 16 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
krmreynolds 0:dc35364e2291 17 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
krmreynolds 0:dc35364e2291 18 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
krmreynolds 0:dc35364e2291 19 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
krmreynolds 0:dc35364e2291 20 * THE SOFTWARE.
krmreynolds 0:dc35364e2291 21 */
krmreynolds 0:dc35364e2291 22 #include <L3G4200D.h>
krmreynolds 1:3272ece36ce1 23 #include <math.h>
krmreynolds 1:3272ece36ce1 24 #include "mbed.h"
krmreynolds 0:dc35364e2291 25
krmreynolds 1:3272ece36ce1 26
ogarai 2:85214374e094 27 L3G4200D::L3G4200D( PinName sda = PTE0, PinName scl = PTE1 ) : i2c( sda, scl ) {
krmreynolds 1:3272ece36ce1 28 writeReg( L3G4200D_CTRL_REG1, 0x0f );
krmreynolds 1:3272ece36ce1 29 }
krmreynolds 0:dc35364e2291 30
krmreynolds 1:3272ece36ce1 31 void L3G4200D::writeReg( byte reg, byte value ) {
krmreynolds 1:3272ece36ce1 32 char data[2] = { reg, value };
krmreynolds 1:3272ece36ce1 33 i2c.write( GYR_ADDRESS, data, 2 );
krmreynolds 1:3272ece36ce1 34 }
krmreynolds 0:dc35364e2291 35
krmreynolds 1:3272ece36ce1 36 char L3G4200D::readReg( byte reg ) {
krmreynolds 1:3272ece36ce1 37 char value[1];
krmreynolds 1:3272ece36ce1 38 char data[1] = { reg };
krmreynolds 1:3272ece36ce1 39 i2c.write( GYR_ADDRESS, data, 1 );
krmreynolds 1:3272ece36ce1 40 i2c.read( GYR_ADDRESS, value, 1 );
krmreynolds 1:3272ece36ce1 41 return value[0];
krmreynolds 0:dc35364e2291 42 }
krmreynolds 0:dc35364e2291 43
krmreynolds 1:3272ece36ce1 44 void L3G4200D::read( void ) {
krmreynolds 1:3272ece36ce1 45 char data[1] = { L3G4200D_OUT_X_L | (1<<7) };
krmreynolds 1:3272ece36ce1 46 char output[6];
krmreynolds 1:3272ece36ce1 47 i2c.write( GYR_ADDRESS, data, 1 );
krmreynolds 0:dc35364e2291 48
krmreynolds 1:3272ece36ce1 49 i2c.read( GYR_ADDRESS, output, 6 );
krmreynolds 0:dc35364e2291 50
krmreynolds 1:3272ece36ce1 51 g.x = short( output[1] << 8 | output[0] );
krmreynolds 1:3272ece36ce1 52 g.y = short( output[3] << 8 | output[2] );
krmreynolds 1:3272ece36ce1 53 g.z = short( output[5] << 8 | output[4] );
krmreynolds 0:dc35364e2291 54 }
krmreynolds 0:dc35364e2291 55
krmreynolds 1:3272ece36ce1 56 void L3G4200D::vector_cross(const Plane *a,const Plane *b, Plane *out) {
krmreynolds 1:3272ece36ce1 57 out->x = a->y*b->z - a->z*b->y;
krmreynolds 1:3272ece36ce1 58 out->y = a->z*b->x - a->x*b->z;
krmreynolds 1:3272ece36ce1 59 out->z = a->x*b->y - a->y*b->x;
krmreynolds 0:dc35364e2291 60 }
krmreynolds 0:dc35364e2291 61
krmreynolds 1:3272ece36ce1 62 float L3G4200D::vector_dot(const Plane *a,const Plane *b) {
krmreynolds 1:3272ece36ce1 63 return a->x*b->x+a->y*b->y+a->z*b->z;
krmreynolds 0:dc35364e2291 64 }
krmreynolds 0:dc35364e2291 65
krmreynolds 1:3272ece36ce1 66 void L3G4200D::vector_normalize(Plane *a) {
krmreynolds 1:3272ece36ce1 67 float mag = sqrt(vector_dot(a,a));
krmreynolds 1:3272ece36ce1 68 a->x /= mag;
krmreynolds 1:3272ece36ce1 69 a->y /= mag;
krmreynolds 1:3272ece36ce1 70 a->z /= mag;
krmreynolds 0:dc35364e2291 71 }