File content as of revision 29:ae765492fa8b:

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

float NUM_SAMPLES = 500;

void initSensors(Quadcopter &quad)
{
    Adafruit_LSM303_Accel_Unified accel = quad.getAccel();
    Adafruit_LSM303_Mag_Unified mag = quad.getMag();
    Adafruit_L3GD20_Unified gyro = quad.getGyro();
    Adafruit_9DOF dof = quad.getIMU();
    offset* initial_offsets = quad.getOffset();

    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 */
    sensors_event_t gyro_event;
    sensors_event_t accel_event;
    sensors_event_t mag_event;
    sensors_vec_t   orientation;
    initial_offsets->gyro_x  = 0;
    initial_offsets->gyro_y  = 0;
    initial_offsets->gyro_z  = 0;
    initial_offsets->roll    = 0;
    initial_offsets->pitch   = 0;
    initial_offsets->heading = 0;

    for (int n = 0; n < NUM_SAMPLES; n++) {
        gyro.getEvent(&gyro_event);
        accel.getEvent(&accel_event);
        dof.accelGetOrientation(&accel_event, &orientation);
        mag.getEvent(&mag_event);
        dof.magGetOrientation(SENSOR_AXIS_Z, &mag_event, &orientation);
        initial_offsets->gyro_x  += gyro_event.gyro.x;
        initial_offsets->gyro_y  += gyro_event.gyro.y;
        initial_offsets->gyro_z  += gyro_event.gyro.z;
        initial_offsets->roll    += orientation.roll;
        initial_offsets->pitch   += orientation.pitch;
        initial_offsets->heading += orientation.heading;
    }
    initial_offsets->gyro_x  /= NUM_SAMPLES;
    initial_offsets->gyro_y  /= NUM_SAMPLES;
    initial_offsets->gyro_z  /= NUM_SAMPLES;
    initial_offsets->roll    /= NUM_SAMPLES;
    initial_offsets->pitch   /= NUM_SAMPLES;
    initial_offsets->heading /= NUM_SAMPLES;
}