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;
}