David Lakata / Mbed 2 deprecated QuadTrio

ESE350 project, Spring 2016, University of Pennsylvania

Dependencies:   Adafruit9-DOf Receiver mbed-rtos mbed

sensor.cpp@46:4bcf2e679f96, 2016-05-05 (annotated)

Committer:
ivo_david_michelle
Date:
Thu May 05 15:58:49 2016 +0000
Revision:
46:4bcf2e679f96
Parent:
37:a983eb9fd9c5 
best stand so far. hovering see video

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 {
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 }