![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
ESE350 project, Spring 2016, University of Pennsylvania
Dependencies: Adafruit9-DOf Receiver mbed-rtos mbed
Diff: sensor.cpp
- Revision:
- 46:4bcf2e679f96
- Parent:
- 37:a983eb9fd9c5
diff -r 9f74298eee78 -r 4bcf2e679f96 sensor.cpp --- 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; + + }