David Lakata / Mbed 2 deprecated QuadTrio

ESE350 project, Spring 2016, University of Pennsylvania

Dependencies:   Adafruit9-DOf Receiver mbed-rtos mbed

sensor.cpp@37:a983eb9fd9c5, 2016-05-01 (annotated)

Committer:
ivo_david_michelle
Date:
Sun May 01 18:45:02 2016 +0000
Revision:
37:a983eb9fd9c5
Parent:
35:35997980a8ba
Child:
46:4bcf2e679f96
david's changes and printing diagnostics from readRC

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 24:e220fbb70ded 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 3:828e82089564 21 if (!mag.begin()) {
ivo_david_michelle 3:828e82089564 22 /* There was a problem detecting the LSM303 ... check your connections */
ivo_david_michelle 3:828e82089564 23 s_com->println("Ooops, no LSM303 mag detected ... Check your wiring!");
ivo_david_michelle 3:828e82089564 24 while(1);
ivo_david_michelle 3:828e82089564 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 3:828e82089564 31 /* Calculate initial offsets and noise level of gyro */
ivo_david_michelle 3:828e82089564 32 sensors_event_t gyro_event;
ivo_david_michelle 24:e220fbb70ded 33 sensors_event_t accel_event;
ivo_david_michelle 24:e220fbb70ded 34 sensors_event_t mag_event;
ivo_david_michelle 24:e220fbb70ded 35 sensors_vec_t orientation;
ivo_david_michelle 24:e220fbb70ded 36 initial_offsets->gyro_x = 0;
ivo_david_michelle 24:e220fbb70ded 37 initial_offsets->gyro_y = 0;
ivo_david_michelle 24:e220fbb70ded 38 initial_offsets->gyro_z = 0;
ivo_david_michelle 24:e220fbb70ded 39 initial_offsets->roll = 0;
ivo_david_michelle 24:e220fbb70ded 40 initial_offsets->pitch = 0;
ivo_david_michelle 24:e220fbb70ded 41 initial_offsets->heading = 0;
ivo_david_michelle 3:828e82089564 42
ivo_david_michelle 24:e220fbb70ded 43 for (int n = 0; n < NUM_SAMPLES; n++) {
ivo_david_michelle 3:828e82089564 44 gyro.getEvent(&gyro_event);
ivo_david_michelle 24:e220fbb70ded 45 accel.getEvent(&accel_event);
ivo_david_michelle 37:a983eb9fd9c5 46 // mag.getEvent(&mag_event);
ivo_david_michelle 37:a983eb9fd9c5 47 dof.accelGetOrientation(&accel_event, &orientation);
ivo_david_michelle 24:e220fbb70ded 48 initial_offsets->gyro_x += gyro_event.gyro.x;
ivo_david_michelle 24:e220fbb70ded 49 initial_offsets->gyro_y += gyro_event.gyro.y;
ivo_david_michelle 24:e220fbb70ded 50 initial_offsets->gyro_z += gyro_event.gyro.z;
ivo_david_michelle 24:e220fbb70ded 51 initial_offsets->roll += orientation.roll;
ivo_david_michelle 24:e220fbb70ded 52 initial_offsets->pitch += orientation.pitch;
ivo_david_michelle 24:e220fbb70ded 53 initial_offsets->heading += orientation.heading;
ivo_david_michelle 3:828e82089564 54 }
ivo_david_michelle 24:e220fbb70ded 55 initial_offsets->gyro_x /= NUM_SAMPLES;
ivo_david_michelle 24:e220fbb70ded 56 initial_offsets->gyro_y /= NUM_SAMPLES;
ivo_david_michelle 24:e220fbb70ded 57 initial_offsets->gyro_z /= NUM_SAMPLES;
ivo_david_michelle 24:e220fbb70ded 58 initial_offsets->roll /= NUM_SAMPLES;
ivo_david_michelle 24:e220fbb70ded 59 initial_offsets->pitch /= NUM_SAMPLES;
ivo_david_michelle 24:e220fbb70ded 60 initial_offsets->heading /= NUM_SAMPLES;
ivo_david_michelle 3:828e82089564 61 }