![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
ESE350 project, Spring 2016, University of Pennsylvania
Dependencies: Adafruit9-DOf Receiver mbed-rtos mbed
sensor.cpp@51:b6d76a4dfae8, 2016-05-07 (annotated)
- Committer:
- ivo_david_michelle
- Date:
- Sat May 07 17:47:22 2016 +0000
- Revision:
- 51:b6d76a4dfae8
- Parent:
- 46:4bcf2e679f96
publishing
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ivo_david_michelle | 3:828e82089564 | 1 | #include "sensor.h" |
ivo_david_michelle | 3:828e82089564 | 2 | #include "Adafruit_9DOF.h" |
ivo_david_michelle | 3:828e82089564 | 3 | #include "Serial_base.h" |
ivo_david_michelle | 3:828e82089564 | 4 | |
ivo_david_michelle | 24:e220fbb70ded | 5 | float NUM_SAMPLES = 500; |
ivo_david_michelle | 3:828e82089564 | 6 | |
ivo_david_michelle | 24:e220fbb70ded | 7 | void initSensors(Quadcopter &quad) |
ivo_david_michelle | 24:e220fbb70ded | 8 | { |
ivo_david_michelle | 24:e220fbb70ded | 9 | Adafruit_LSM303_Accel_Unified accel = quad.getAccel(); |
ivo_david_michelle | 46:4bcf2e679f96 | 10 | //Adafruit_LSM303_Mag_Unified mag = quad.getMag(); |
ivo_david_michelle | 24:e220fbb70ded | 11 | Adafruit_L3GD20_Unified gyro = quad.getGyro(); |
ivo_david_michelle | 24:e220fbb70ded | 12 | Adafruit_9DOF dof = quad.getIMU(); |
ivo_david_michelle | 24:e220fbb70ded | 13 | offset* initial_offsets = quad.getOffset(); |
ivo_david_michelle | 3:828e82089564 | 14 | |
ivo_david_michelle | 4:3040d0f9e8c6 | 15 | s_com->println(("\r\nInitializing sensors!\n\r")); |
ivo_david_michelle | 3:828e82089564 | 16 | if (!accel.begin()) { |
ivo_david_michelle | 3:828e82089564 | 17 | /* There was a problem detecting the LSM303 ... check your connections */ |
ivo_david_michelle | 3:828e82089564 | 18 | s_com->println(("Ooops, no LSM303 accel detected ... Check your wiring!")); |
ivo_david_michelle | 3:828e82089564 | 19 | while(1); |
ivo_david_michelle | 3:828e82089564 | 20 | } |
ivo_david_michelle | 46:4bcf2e679f96 | 21 | // if (!mag.begin()) { |
ivo_david_michelle | 46:4bcf2e679f96 | 22 | // /* There was a problem detecting the LSM303 ... check your connections */ |
ivo_david_michelle | 46:4bcf2e679f96 | 23 | // s_com->println("Ooops, no LSM303 mag detected ... Check your wiring!"); |
ivo_david_michelle | 46:4bcf2e679f96 | 24 | // while(1); |
ivo_david_michelle | 46:4bcf2e679f96 | 25 | // } |
ivo_david_michelle | 3:828e82089564 | 26 | if (!gyro.begin(GYRO_RANGE_2000DPS)) { |
ivo_david_michelle | 3:828e82089564 | 27 | /* There was a problem detecting the L3GD20 ... check your connections */ |
ivo_david_michelle | 3:828e82089564 | 28 | s_com->println("Ooops, no L3GD20 gyro detected ... Check your wiring or I2C ADDR!"); |
ivo_david_michelle | 3:828e82089564 | 29 | while(1); |
ivo_david_michelle | 3:828e82089564 | 30 | } |
ivo_david_michelle | 46:4bcf2e679f96 | 31 | |
ivo_david_michelle | 46:4bcf2e679f96 | 32 | |
ivo_david_michelle | 3:828e82089564 | 33 | /* Calculate initial offsets and noise level of gyro */ |
ivo_david_michelle | 46:4bcf2e679f96 | 34 | // sensors_event_t gyro_event; |
ivo_david_michelle | 46:4bcf2e679f96 | 35 | // sensors_event_t accel_event; |
ivo_david_michelle | 46:4bcf2e679f96 | 36 | // //sensors_event_t mag_event; |
ivo_david_michelle | 46:4bcf2e679f96 | 37 | // sensors_vec_t orientation; |
ivo_david_michelle | 46:4bcf2e679f96 | 38 | // initial_offsets->gyro_x = 0; |
ivo_david_michelle | 46:4bcf2e679f96 | 39 | // initial_offsets->gyro_y = 0; |
ivo_david_michelle | 46:4bcf2e679f96 | 40 | // initial_offsets->gyro_z = 0; |
ivo_david_michelle | 46:4bcf2e679f96 | 41 | // initial_offsets->roll = 0; |
ivo_david_michelle | 46:4bcf2e679f96 | 42 | // initial_offsets->pitch = 0; |
ivo_david_michelle | 46:4bcf2e679f96 | 43 | // initial_offsets->heading = 0; |
ivo_david_michelle | 3:828e82089564 | 44 | |
ivo_david_michelle | 46:4bcf2e679f96 | 45 | // for (int n = 0; n < NUM_SAMPLES; n++) { |
ivo_david_michelle | 46:4bcf2e679f96 | 46 | // gyro.getEvent(&gyro_event); |
ivo_david_michelle | 46:4bcf2e679f96 | 47 | // accel.getEvent(&accel_event); |
ivo_david_michelle | 46:4bcf2e679f96 | 48 | //// mag.getEvent(&mag_event); |
ivo_david_michelle | 46:4bcf2e679f96 | 49 | // dof.accelGetOrientation(&accel_event, &orientation); |
ivo_david_michelle | 46:4bcf2e679f96 | 50 | // initial_offsets->gyro_x += gyro_event.gyro.x; |
ivo_david_michelle | 46:4bcf2e679f96 | 51 | // initial_offsets->gyro_y += gyro_event.gyro.y; |
ivo_david_michelle | 46:4bcf2e679f96 | 52 | // initial_offsets->gyro_z += gyro_event.gyro.z; |
ivo_david_michelle | 46:4bcf2e679f96 | 53 | // initial_offsets->roll += orientation.roll; |
ivo_david_michelle | 46:4bcf2e679f96 | 54 | // initial_offsets->pitch += orientation.pitch; |
ivo_david_michelle | 46:4bcf2e679f96 | 55 | // initial_offsets->heading += orientation.heading; |
ivo_david_michelle | 46:4bcf2e679f96 | 56 | // } |
ivo_david_michelle | 46:4bcf2e679f96 | 57 | // initial_offsets->gyro_x /= NUM_SAMPLES; |
ivo_david_michelle | 46:4bcf2e679f96 | 58 | // initial_offsets->gyro_y /= NUM_SAMPLES; |
ivo_david_michelle | 46:4bcf2e679f96 | 59 | // initial_offsets->gyro_z /= NUM_SAMPLES; |
ivo_david_michelle | 46:4bcf2e679f96 | 60 | // initial_offsets->roll /= NUM_SAMPLES; |
ivo_david_michelle | 46:4bcf2e679f96 | 61 | // initial_offsets->pitch /= NUM_SAMPLES; |
ivo_david_michelle | 46:4bcf2e679f96 | 62 | // initial_offsets->heading /= NUM_SAMPLES; |
ivo_david_michelle | 46:4bcf2e679f96 | 63 | |
ivo_david_michelle | 46:4bcf2e679f96 | 64 | |
ivo_david_michelle | 3:828e82089564 | 65 | } |