Diff: main.cpp

Revision:

5:4af75af374cc

Parent:

4:4b6f0c4cd39f

--- a/main.cpp	Mon Nov 19 03:56:17 2018 +0000
+++ b/main.cpp	Thu Nov 22 22:40:15 2018 +0000
@@ -21,6 +21,8 @@
     struct vector xUnit = {1, 0, 0};
     struct vector yUnit = {0, 1, 0};
     struct vector zUnit = {0, 0, 1};
+    struct vector filteredGyro= {0,0,0};
+    struct vector filteredAccel = {0,0,0};
     struct quaternion xRot;
     struct quaternion yRot;
     struct quaternion zRot;
@@ -29,11 +31,17 @@
     float time_previous = 0;
     float delta_time = 0;
     float radFactor = (3.14159265358979323846/180)/15.3;
+    float gyroFilterConstant = 0.0025;
+    float accelFilterConstant = 15000;
+    float accelNegativeFilterConstant = -15000;
     struct vector gAngularV = {0, 0, 0};
+    struct vector finalO;
+    float alpha = 0.3;
     
     
     while(1) {
         if (!mpu.data_ready()) {
+            //pc.printf("%b", mpu.data_ready());
             continue;
         }
         
@@ -44,7 +52,7 @@
         
         //bias
         struct vector gyro_bias = {0, 0, 0};
-        struct vector accel_bias = {500, 350, 2000};
+        struct vector accel_bias = {1300, -500, 16500};
         
         //gets raw data
         mpu.read_raw(&gyro.x, &gyro.y, &gyro.z, &accel.x, &accel.y, &accel.z);
@@ -56,7 +64,7 @@
         time_current = (float(millis()) / 1000);
         delta_time = time_current - time_previous;
         vector_add(&gyro, &gyro_previous, &gyro_avg);
-        vector_multiply(&gyro_avg, 0.5, &gAngularV);
+        vector_multiply(&gyro_avg, -0.5, &gAngularV);
         vector_multiply(&gAngularV, delta_time, &gAngularV);
         gyro_previous = gyro;
         time_previous = time_current;
@@ -64,27 +72,88 @@
         //rotate orientation vector
         gAngularV.x *= radFactor;
         gAngularV.y *= radFactor;
-        gAngularV.z *= radFactor;
+        gAngularV.z *= radFactor;        
+        
+        
+        //pc.printf("%f %f %f\r\n", gAngularV.x,gAngularV.y,gAngularV.z);
+        //wait(0.1);
         quaternion_create(&xUnit, gAngularV.x, &xRot);
         quaternion_create(&yUnit, gAngularV.y, &yRot);
         quaternion_create(&zUnit, gAngularV.z, &zRot);
         quaternion_multiply(&xRot, &yRot, &finalRot);
         quaternion_multiply(&finalRot, &zRot, &finalRot);
         quaternion_rotate(&orientation, &finalRot, &orientation);
-        vector_normalize(&orientation, &orientation);
         
-        //adds bias
+         //adds bias
         vector_add(&accel, &accel_bias, &accel);
         vector_add(&gyro, &gyro_bias, &gyro);
+        
+        //create filtered vectors
+        filteredGyro= orientation;
+        filteredAccel = accel;
+        
+        vector_normalize(&orientation, &orientation);
+        
+       
         //pc.printf( "%f %f %f\r\n", accel.x, accel.y, accel.z);
         //pc.printf("%f %f %f\r\n",  gyro.x, gyro.y, gyro.z);
-        wait(0.1);
+        //wait(0.1);
+        
+       
+        
+        //Filter the change in angle so low values don't pass
+         if(fabs(gAngularV.x) < gyroFilterConstant)
+        {
+            filteredGyro.x = 0;
+        }
+        if(fabs(gAngularV.y) < gyroFilterConstant)
+        {
+            filteredGyro.y = 0;
+        }
+        if(fabs(gAngularV.z) < gyroFilterConstant)
+        {
+            filteredGyro.z = 0;
+        }
 
+        //Filter the accel data so high values don't pass
+        if(accel.x > accelFilterConstant)
+        {
+            filteredAccel.x = accelFilterConstant;
+        }
+        if(accel.y > accelFilterConstant)
+        {
+            filteredAccel.y = accelFilterConstant;
+        }
+        if(accel.z > accelFilterConstant)
+        {
+            filteredAccel.z = accelFilterConstant;
+        }
+        
+        //filter the negative high values
+        if(accel.x < accelNegativeFilterConstant)
+        {
+            filteredAccel.x = accelNegativeFilterConstant;
+        }
+        if(accel.y < accelNegativeFilterConstant)
+        {
+            filteredAccel.y = accelNegativeFilterConstant;
+        }
+        if(accel.z < accelNegativeFilterConstant)
+        {
+            filteredAccel.z = accelNegativeFilterConstant;
+        }
+        
+        //calculate filtered orientation
+        vector_multiply(&filteredAccel, alpha, &filteredAccel);
+        vector_multiply(&filteredGyro, (1 - alpha), &filteredGyro);
+        vector_add(&filteredAccel, &filteredGyro, &finalO);
+        vector_normalize(&finalO, &finalO);
+        
         //normalize raw data to get vector
         struct vector accel_norm = {0, 0, 0};
         vector_normalize(&accel, &accel_norm);
-        pc.printf("%f %f %f\r\n",  orientation.x, orientation.y, orientation.z);
+        pc.printf("%f %f %f %f %f %f %f %f %f\r\n", accel_norm.x,accel_norm.y,accel_norm.z, orientation.x, orientation.y, orientation.z, finalO.x, finalO.y, finalO.z);
         
-        //wait(1);
+        wait(0.1);
     }
 }