David Lakata / Mbed 2 deprecated QuadTrio

ESE350 project, Spring 2016, University of Pennsylvania

Dependencies:   Adafruit9-DOf Receiver mbed-rtos mbed

sensor.cpp@14:64b06476d943, 2016-04-07 (annotated)

Committer:
ivo_david_michelle
Date:
Thu Apr 07 02:07:33 2016 +0000
Revision:
14:64b06476d943
Parent:
10:e7d1801e966a
Child:
23:04338a5ef404
msoch update - compiles!

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 3:828e82089564 5 /* Offsets of gyro at rest */
ivo_david_michelle 3:828e82089564 6 //float x_offset = 0, y_offset = 0, z_offset = 0;
ivo_david_michelle 3:828e82089564 7
ivo_david_michelle 3:828e82089564 8
ivo_david_michelle 3:828e82089564 9 void initSensors(Adafruit_LSM303_Accel_Unified &accel,Adafruit_LSM303_Mag_Unified &mag,Adafruit_L3GD20_Unified &gyro, offset &offset_gyro)
ivo_david_michelle 3:828e82089564 10 {
ivo_david_michelle 4:3040d0f9e8c6 11 s_com->println(("\r\nInitializing sensors!\n\r"));
ivo_david_michelle 3:828e82089564 12 if (!accel.begin()) {
ivo_david_michelle 3:828e82089564 13 /* There was a problem detecting the LSM303 ... check your connections */
ivo_david_michelle 3:828e82089564 14 s_com->println(("Ooops, no LSM303 accel detected ... Check your wiring!"));
ivo_david_michelle 3:828e82089564 15 while(1);
ivo_david_michelle 3:828e82089564 16 }
ivo_david_michelle 3:828e82089564 17 if (!mag.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 mag 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 (!gyro.begin(GYRO_RANGE_2000DPS)) {
ivo_david_michelle 3:828e82089564 23 /* There was a problem detecting the L3GD20 ... check your connections */
ivo_david_michelle 3:828e82089564 24 s_com->println("Ooops, no L3GD20 gyro detected ... Check your wiring or I2C ADDR!");
ivo_david_michelle 3:828e82089564 25 while(1);
ivo_david_michelle 3:828e82089564 26 }
ivo_david_michelle 3:828e82089564 27 /* Calculate initial offsets and noise level of gyro */
ivo_david_michelle 3:828e82089564 28 float sampleNum = 500;
ivo_david_michelle 3:828e82089564 29 sensors_event_t gyro_event;
ivo_david_michelle 10:e7d1801e966a 30 offset_gyro.x=0;
ivo_david_michelle 10:e7d1801e966a 31 offset_gyro.y=0;
ivo_david_michelle 10:e7d1801e966a 32 offset_gyro.z=0;
ivo_david_michelle 3:828e82089564 33
ivo_david_michelle 3:828e82089564 34 for (int n = 0; n < sampleNum; n++) {
ivo_david_michelle 3:828e82089564 35 gyro.getEvent(&gyro_event);
ivo_david_michelle 10:e7d1801e966a 36 offset_gyro.x += gyro_event.gyro.x;
ivo_david_michelle 10:e7d1801e966a 37 offset_gyro.y += gyro_event.gyro.y;
ivo_david_michelle 10:e7d1801e966a 38 offset_gyro.z += gyro_event.gyro.z;
ivo_david_michelle 3:828e82089564 39 }
ivo_david_michelle 10:e7d1801e966a 40 offset_gyro.x = offset_gyro.x / sampleNum;
ivo_david_michelle 10:e7d1801e966a 41 offset_gyro.y = offset_gyro.y / sampleNum;
ivo_david_michelle 10:e7d1801e966a 42 offset_gyro.z = offset_gyro.z / sampleNum;
ivo_david_michelle 3:828e82089564 43 /* s_com->print("Offsets... X: ");
ivo_david_michelle 3:828e82089564 44 s_com->print(x_offset);
ivo_david_michelle 3:828e82089564 45 s_com->print("\tY: ");
ivo_david_michelle 3:828e82089564 46 s_com->print(y_offset);
ivo_david_michelle 3:828e82089564 47 s_com->print("\tZ: ");
ivo_david_michelle 3:828e82089564 48 s_com->print(z_offset); */
ivo_david_michelle 3:828e82089564 49 }