Diff: sensor.cpp

Revision:

3:828e82089564

Child:

4:3040d0f9e8c6

--- /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); */
+}