Diff: modSensData.h

Revision:

2:503a5ac6c3b6

Parent:

0:e79311aae7ed

Child:

3:ffa0e1429a72

--- a/modSensData.h	Sat Feb 21 01:22:52 2015 +0000
+++ b/modSensData.h	Wed Mar 04 17:48:08 2015 +0000
@@ -17,6 +17,8 @@
 
 #define GRAVITY       1.0f
 
+#define THRESHOLD 1.1f
+
 Timer t1;
 
 float vC[3], cC[3], magnetom[3], inertialAccel[3];
@@ -178,10 +180,30 @@
 
 
 
-void updateAngles()
+int updateAngles()
 {
     readIMU();
-    getAttitude();
-    Compass_Heading();    
+    if((accel[0] <= THRESHOLD) && (accel[1] <= THRESHOLD) && (accel[2] <= THRESHOLD))
+    {
+        //float prevYaw = yaw;
+        getAttitude();
+        Compass_Heading();
+        //float absOfYawDiff = sqrt((prevYaw - yaw) * (prevYaw - yaw));    
+        //check for noise
+        //if((absOfYawDiff >= 30) && (absOfYawDiff <= (360.0f - 30)))
+        //{
+            return 1;
+        //}
+        //return 0;
+        
+    }
+    return 0;
     //rotateBTI();
+}
+
+float getGyro()
+{
+    float gx, gy, gz;
+    gyro.read(&gx,&gy,&gz);
+    return gz;
 }
\ No newline at end of file