Diff: main.cpp

Revision:

4:4b6f0c4cd39f

Parent:

3:461a9012682d

Child:

5:4af75af374cc

--- a/main.cpp	Mon Nov 19 01:28:14 2018 +0000
+++ b/main.cpp	Mon Nov 19 03:56:17 2018 +0000
@@ -15,6 +15,22 @@
     mpu.start();
     millis_begin();
     struct vector orientation = {0, 0, 1}; //starting point
+    struct vector gyro_previous = {0, 0, 0};
+    struct vector gyro = {0, 0, 0};
+    struct vector gyro_avg;
+    struct vector xUnit = {1, 0, 0};
+    struct vector yUnit = {0, 1, 0};
+    struct vector zUnit = {0, 0, 1};
+    struct quaternion xRot;
+    struct quaternion yRot;
+    struct quaternion zRot;
+    struct quaternion finalRot;
+    float time_current = 0;
+    float time_previous = 0;
+    float delta_time = 0;
+    float radFactor = (3.14159265358979323846/180)/15.3;
+    struct vector gAngularV = {0, 0, 0};
+    
     
     while(1) {
         if (!mpu.data_ready()) {
@@ -23,41 +39,51 @@
         
         //raw data
         struct vector accel;
-        struct vector gyro;
+
+
         
         //bias
         struct vector gyro_bias = {0, 0, 0};
         struct vector accel_bias = {500, 350, 2000};
         
         //gets raw data
-        double time = millis();
         mpu.read_raw(&gyro.x, &gyro.y, &gyro.z, &accel.x, &accel.y, &accel.z);
-        double delta_time = millis() - time; //gets time taken to turn
         //pc.printf("Accel: (%f, %f, %f)\n Gyro: (%f, %f, %f)\r\n", accel.x, accel.y, accel.z, gyro.x, gyro.y, gyro.z);
         //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);
         
+        //calculate angle
+        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(&gAngularV, delta_time, &gAngularV);
+        gyro_previous = gyro;
+        time_previous = time_current;
+        
+        //rotate orientation vector
+        gAngularV.x *= radFactor;
+        gAngularV.y *= radFactor;
+        gAngularV.z *= radFactor;
+        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
         vector_add(&accel, &accel_bias, &accel);
         vector_add(&gyro, &gyro_bias, &gyro);
         //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);
-        
+        //pc.printf("%f %f %f\r\n",  gyro.x, gyro.y, gyro.z);
+        wait(0.1);
+
         //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", accel_norm.x, accel_norm.y, accel_norm.z);
-        wait(0.1);
-        
-        //create a quaternion
-        struct vector gyro_norm;
-        float angle = vector_normalize(&gyro, &gyro_norm);
-        
-        struct vector orientation = {0, 0, 1};
-        quaternion quat;
-        quaternion_create(&gyro_norm, angle, &quat);
+        pc.printf("%f %f %f\r\n",  orientation.x, orientation.y, orientation.z);
         
         //wait(1);
     }