Diff: sensor.cpp

Revision:

46:4bcf2e679f96

Parent:

37:a983eb9fd9c5

--- a/sensor.cpp	Thu May 05 14:42:30 2016 +0000
+++ b/sensor.cpp	Thu May 05 15:58:49 2016 +0000
@@ -7,7 +7,7 @@
 void initSensors(Quadcopter &quad)
 {
     Adafruit_LSM303_Accel_Unified accel = quad.getAccel();
-    Adafruit_LSM303_Mag_Unified mag = quad.getMag();
+    //Adafruit_LSM303_Mag_Unified mag = quad.getMag();
     Adafruit_L3GD20_Unified gyro = quad.getGyro();
     Adafruit_9DOF dof = quad.getIMU();
     offset* initial_offsets = quad.getOffset();
@@ -18,44 +18,48 @@
         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 (!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;
+//    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;
+//    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;
+
+
 }