ESE350 project, Spring 2016, University of Pennsylvania
Dependencies: Adafruit9-DOf Receiver mbed-rtos mbed
Diff: sensor.cpp
- Revision:
- 3:828e82089564
- Child:
- 4:3040d0f9e8c6
diff -r c041e434eab6 -r 828e82089564 sensor.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/sensor.cpp Thu Mar 31 22:05:57 2016 +0000 @@ -0,0 +1,50 @@ +#include "sensor.h" +#include "Adafruit_9DOF.h" +#include "Serial_base.h" + + +/* Offsets of gyro at rest */ +//float x_offset = 0, y_offset = 0, z_offset = 0; + + +void initSensors(Adafruit_LSM303_Accel_Unified &accel,Adafruit_LSM303_Mag_Unified &mag,Adafruit_L3GD20_Unified &gyro, offset &offset_gyro) +{ + s_com->println(("\r\nInitializing sensors!")); + if (!accel.begin()) { + /* There was a problem detecting the LSM303 ... check your connections */ + s_com->println(("Ooops, no LSM303 accel detected ... Check your wiring!")); + while(1); + } + if (!mag.begin()) { + /* There was a problem detecting the LSM303 ... check your connections */ + s_com->println("Ooops, no LSM303 mag detected ... Check your wiring!"); + while(1); + } + if (!gyro.begin(GYRO_RANGE_2000DPS)) { + /* There was a problem detecting the L3GD20 ... check your connections */ + s_com->println("Ooops, no L3GD20 gyro detected ... Check your wiring or I2C ADDR!"); + while(1); + } + /* Calculate initial offsets and noise level of gyro */ + float sampleNum = 500; + sensors_event_t gyro_event; + offset_gyro.x_offset=0; + offset_gyro.y_offset=0; + offset_gyro.z_offset=0; + + for (int n = 0; n < sampleNum; n++) { + gyro.getEvent(&gyro_event); + offset_gyro.x_offset += gyro_event.gyro.x; + offset_gyro.y_offset += gyro_event.gyro.y; + offset_gyro.z_offset += gyro_event.gyro.z; + } + offset_gyro.x_offset = offset_gyro.x_offset / sampleNum; + offset_gyro.y_offset = offset_gyro.y_offset / sampleNum; + offset_gyro.y_offset = offset_gyro.y_offset / sampleNum; + /* s_com->print("Offsets... X: "); + s_com->print(x_offset); + s_com->print("\tY: "); + s_com->print(y_offset); + s_com->print("\tZ: "); + s_com->print(z_offset); */ +}