David Lakata / Mbed 2 deprecated QuadTrio

ESE350 project, Spring 2016, University of Pennsylvania

Dependencies:   Adafruit9-DOf Receiver mbed-rtos mbed

sensor.cpp@3:828e82089564, 2016-03-31 (annotated)

Committer:
ivo_david_michelle
Date:
Thu Mar 31 22:05:57 2016 +0000
Revision:
3:828e82089564
Child:
4:3040d0f9e8c6
more refactoring of code

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