File content as of revision 46:4bcf2e679f96:

#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);
////        mag.getEvent(&mag_event);
//        dof.accelGetOrientation(&accel_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;


}