ESE350 project, Spring 2016, University of Pennsylvania
Dependencies: Adafruit9-DOf Receiver mbed-rtos mbed
Diff: sensor.cpp
- Revision:
- 24:e220fbb70ded
- Parent:
- 23:04338a5ef404
- Child:
- 29:ae765492fa8b
diff -r 04338a5ef404 -r e220fbb70ded sensor.cpp --- a/sensor.cpp Tue Apr 12 22:54:16 2016 +0000 +++ b/sensor.cpp Wed Apr 13 00:57:15 2016 +0000 @@ -2,12 +2,17 @@ #include "Adafruit_9DOF.h" #include "Serial_base.h" -/* Offsets of gyro at rest */ -//float x_offset = 0, y_offset = 0, z_offset = 0; +float NUM_SAMPLES = 500; +void initSensors(Quadcopter &quad) +//void initSensors(Adafruit_LSM303_Accel_Unified &accel,Adafruit_LSM303_Mag_Unified &mag,Adafruit_L3GD20_Unified &gyro, offset &offset_gyro) +{ + Adafruit_LSM303_Accel_Unified accel = quad.getAccel(); + Adafruit_LSM303_Mag_Unified mag = quad.getMag(); + Adafruit_L3GD20_Unified gyro = quad.getGyro(); + Adafruit_9DOF dof = quad.getIMU(); + offset* initial_offsets = quad.getOffset(); -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!\n\r")); if (!accel.begin()) { /* There was a problem detecting the LSM303 ... check your connections */ @@ -25,19 +30,34 @@ while(1); } /* Calculate initial offsets and noise level of gyro */ - float sampleNum = 500; sensors_event_t gyro_event; - offset_gyro.x=0; - offset_gyro.y=0; - offset_gyro.z=0; + sensors_event_t accel_event; + sensors_event_t mag_event; + sensors_vec_t orientation; + initial_offsets->gyro_x = 0; + initial_offsets->gyro_y = 0; + initial_offsets->gyro_z = 0; + initial_offsets->roll = 0; + initial_offsets->pitch = 0; + initial_offsets->heading = 0; - for (int n = 0; n < sampleNum; n++) { + for (int n = 0; n < NUM_SAMPLES; n++) { gyro.getEvent(&gyro_event); - offset_gyro.x += gyro_event.gyro.x; - offset_gyro.y += gyro_event.gyro.y; - offset_gyro.z += gyro_event.gyro.z; + accel.getEvent(&accel_event); + dof.accelGetOrientation(&accel_event, &orientation); + mag.getEvent(&mag_event); + dof.magGetOrientation(SENSOR_AXIS_Z, &mag_event, &orientation); + initial_offsets->gyro_x += gyro_event.gyro.x; + initial_offsets->gyro_y += gyro_event.gyro.y; + initial_offsets->gyro_z += gyro_event.gyro.z; + initial_offsets->roll += orientation.roll; + initial_offsets->pitch += orientation.pitch; + initial_offsets->heading += orientation.heading; } - offset_gyro.x = offset_gyro.x / sampleNum; - offset_gyro.y = offset_gyro.y / sampleNum; - offset_gyro.z = offset_gyro.z / sampleNum; + initial_offsets->gyro_x /= NUM_SAMPLES; + initial_offsets->gyro_y /= NUM_SAMPLES; + initial_offsets->gyro_z /= NUM_SAMPLES; + initial_offsets->roll /= NUM_SAMPLES; + initial_offsets->pitch /= NUM_SAMPLES; + initial_offsets->heading /= NUM_SAMPLES; }