ESE350 project, Spring 2016, University of Pennsylvania
Dependencies: Adafruit9-DOf Receiver mbed-rtos mbed
sensor.cpp@24:e220fbb70ded, 2016-04-13 (annotated)
- Committer:
- ivo_david_michelle
- Date:
- Wed Apr 13 00:57:15 2016 +0000
- Revision:
- 24:e220fbb70ded
- Parent:
- 23:04338a5ef404
- Child:
- 29:ae765492fa8b
after integration
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 | //void initSensors(Adafruit_LSM303_Accel_Unified &accel,Adafruit_LSM303_Mag_Unified &mag,Adafruit_L3GD20_Unified &gyro, offset &offset_gyro) |
ivo_david_michelle | 24:e220fbb70ded | 9 | { |
ivo_david_michelle | 24:e220fbb70ded | 10 | Adafruit_LSM303_Accel_Unified accel = quad.getAccel(); |
ivo_david_michelle | 24:e220fbb70ded | 11 | Adafruit_LSM303_Mag_Unified mag = quad.getMag(); |
ivo_david_michelle | 24:e220fbb70ded | 12 | Adafruit_L3GD20_Unified gyro = quad.getGyro(); |
ivo_david_michelle | 24:e220fbb70ded | 13 | Adafruit_9DOF dof = quad.getIMU(); |
ivo_david_michelle | 24:e220fbb70ded | 14 | offset* initial_offsets = quad.getOffset(); |
ivo_david_michelle | 3:828e82089564 | 15 | |
ivo_david_michelle | 4:3040d0f9e8c6 | 16 | s_com->println(("\r\nInitializing sensors!\n\r")); |
ivo_david_michelle | 3:828e82089564 | 17 | if (!accel.begin()) { |
ivo_david_michelle | 3:828e82089564 | 18 | /* There was a problem detecting the LSM303 ... check your connections */ |
ivo_david_michelle | 3:828e82089564 | 19 | s_com->println(("Ooops, no LSM303 accel detected ... Check your wiring!")); |
ivo_david_michelle | 3:828e82089564 | 20 | while(1); |
ivo_david_michelle | 3:828e82089564 | 21 | } |
ivo_david_michelle | 3:828e82089564 | 22 | if (!mag.begin()) { |
ivo_david_michelle | 3:828e82089564 | 23 | /* There was a problem detecting the LSM303 ... check your connections */ |
ivo_david_michelle | 3:828e82089564 | 24 | s_com->println("Ooops, no LSM303 mag detected ... Check your wiring!"); |
ivo_david_michelle | 3:828e82089564 | 25 | while(1); |
ivo_david_michelle | 3:828e82089564 | 26 | } |
ivo_david_michelle | 3:828e82089564 | 27 | if (!gyro.begin(GYRO_RANGE_2000DPS)) { |
ivo_david_michelle | 3:828e82089564 | 28 | /* There was a problem detecting the L3GD20 ... check your connections */ |
ivo_david_michelle | 3:828e82089564 | 29 | s_com->println("Ooops, no L3GD20 gyro detected ... Check your wiring or I2C ADDR!"); |
ivo_david_michelle | 3:828e82089564 | 30 | while(1); |
ivo_david_michelle | 3:828e82089564 | 31 | } |
ivo_david_michelle | 3:828e82089564 | 32 | /* Calculate initial offsets and noise level of gyro */ |
ivo_david_michelle | 3:828e82089564 | 33 | sensors_event_t gyro_event; |
ivo_david_michelle | 24:e220fbb70ded | 34 | sensors_event_t accel_event; |
ivo_david_michelle | 24:e220fbb70ded | 35 | sensors_event_t mag_event; |
ivo_david_michelle | 24:e220fbb70ded | 36 | sensors_vec_t orientation; |
ivo_david_michelle | 24:e220fbb70ded | 37 | initial_offsets->gyro_x = 0; |
ivo_david_michelle | 24:e220fbb70ded | 38 | initial_offsets->gyro_y = 0; |
ivo_david_michelle | 24:e220fbb70ded | 39 | initial_offsets->gyro_z = 0; |
ivo_david_michelle | 24:e220fbb70ded | 40 | initial_offsets->roll = 0; |
ivo_david_michelle | 24:e220fbb70ded | 41 | initial_offsets->pitch = 0; |
ivo_david_michelle | 24:e220fbb70ded | 42 | initial_offsets->heading = 0; |
ivo_david_michelle | 3:828e82089564 | 43 | |
ivo_david_michelle | 24:e220fbb70ded | 44 | for (int n = 0; n < NUM_SAMPLES; n++) { |
ivo_david_michelle | 3:828e82089564 | 45 | gyro.getEvent(&gyro_event); |
ivo_david_michelle | 24:e220fbb70ded | 46 | accel.getEvent(&accel_event); |
ivo_david_michelle | 24:e220fbb70ded | 47 | dof.accelGetOrientation(&accel_event, &orientation); |
ivo_david_michelle | 24:e220fbb70ded | 48 | mag.getEvent(&mag_event); |
ivo_david_michelle | 24:e220fbb70ded | 49 | dof.magGetOrientation(SENSOR_AXIS_Z, &mag_event, &orientation); |
ivo_david_michelle | 24:e220fbb70ded | 50 | initial_offsets->gyro_x += gyro_event.gyro.x; |
ivo_david_michelle | 24:e220fbb70ded | 51 | initial_offsets->gyro_y += gyro_event.gyro.y; |
ivo_david_michelle | 24:e220fbb70ded | 52 | initial_offsets->gyro_z += gyro_event.gyro.z; |
ivo_david_michelle | 24:e220fbb70ded | 53 | initial_offsets->roll += orientation.roll; |
ivo_david_michelle | 24:e220fbb70ded | 54 | initial_offsets->pitch += orientation.pitch; |
ivo_david_michelle | 24:e220fbb70ded | 55 | initial_offsets->heading += orientation.heading; |
ivo_david_michelle | 3:828e82089564 | 56 | } |
ivo_david_michelle | 24:e220fbb70ded | 57 | initial_offsets->gyro_x /= NUM_SAMPLES; |
ivo_david_michelle | 24:e220fbb70ded | 58 | initial_offsets->gyro_y /= NUM_SAMPLES; |
ivo_david_michelle | 24:e220fbb70ded | 59 | initial_offsets->gyro_z /= NUM_SAMPLES; |
ivo_david_michelle | 24:e220fbb70ded | 60 | initial_offsets->roll /= NUM_SAMPLES; |
ivo_david_michelle | 24:e220fbb70ded | 61 | initial_offsets->pitch /= NUM_SAMPLES; |
ivo_david_michelle | 24:e220fbb70ded | 62 | initial_offsets->heading /= NUM_SAMPLES; |
ivo_david_michelle | 3:828e82089564 | 63 | } |