Diff: sensor.cpp

Revision:

24:e220fbb70ded

Parent:

23:04338a5ef404

Child:

29:ae765492fa8b

--- 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;
 }