David Lakata / Mbed 2 deprecated QuadTrio

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?

UserRevisionLine numberNew 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 }