File content as of revision 14:64b06476d943:

#include "sensor.h"
#include "Adafruit_9DOF.h"
#include "Serial_base.h"

/* Offsets of gyro at rest */
//float x_offset = 0, y_offset = 0, z_offset = 0;


void initSensors(Adafruit_LSM303_Accel_Unified &accel,Adafruit_LSM303_Mag_Unified &mag,Adafruit_L3GD20_Unified &gyro, offset &offset_gyro)
{
    s_com->println(("\r\nInitializing sensors!\n\r"));
    if (!accel.begin()) {
        /* There was a problem detecting the LSM303 ... check your connections */
        s_com->println(("Ooops, no LSM303 accel detected ... Check your wiring!"));
        while(1);
    }
    if (!mag.begin()) {
        /* There was a problem detecting the LSM303 ... check your connections */
        s_com->println("Ooops, no LSM303 mag detected ... Check your wiring!");
        while(1);
    }
    if (!gyro.begin(GYRO_RANGE_2000DPS)) {
        /* There was a problem detecting the L3GD20 ... check your connections */
        s_com->println("Ooops, no L3GD20 gyro detected ... Check your wiring or I2C ADDR!");
        while(1);
    }
    /* Calculate initial offsets and noise level of gyro */
    float sampleNum = 500;
    sensors_event_t gyro_event;
    offset_gyro.x=0;
    offset_gyro.y=0;
    offset_gyro.z=0;

    for (int n = 0; n < sampleNum; n++) {
        gyro.getEvent(&gyro_event);
        offset_gyro.x += gyro_event.gyro.x;
        offset_gyro.y += gyro_event.gyro.y;
        offset_gyro.z += gyro_event.gyro.z;
    }
    offset_gyro.x = offset_gyro.x / sampleNum;
    offset_gyro.y = offset_gyro.y / sampleNum;
    offset_gyro.z = offset_gyro.z / sampleNum;
  /*  s_com->print("Offsets... X: ");
    s_com->print(x_offset);
    s_com->print("\tY: ");
    s_com->print(y_offset);
    s_com->print("\tZ: ");
    s_com->print(z_offset); */
}