Alvee Ahmed / virgo3_imuHandler

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Fork of virgo3_imuHandler_Orion_PCB by Van Nguyen

Diff: imuHandler.cpp

Revision:
8:b3b784689d87
Parent:
7:132bd7a919f5
Child:
9:47b6a8530868
--- a/imuHandler.cpp	Mon May 16 08:26:24 2016 +0000
+++ b/imuHandler.cpp	Tue May 17 05:42:48 2016 +0000
@@ -145,16 +145,16 @@
 
             //moving window average of pose angle
             for(int i=0; i<3; i++) {
-                movWindow_Pose[i][movWindow_index_Pose] = sin(imu_Data.posePRY[i]);
-                Pose[i] = asin(generalFunctions::moving_window(movWindow_Pose[i], movWindow_len_Pose));
+                movWindow_Pose[i][movWindow_index_Pose] = sin(DEG_TO_RAD(imu_Data.posePRY[i]));
+                Pose[i] = RAD_TO_DEG(asin(generalFunctions::moving_window(movWindow_Pose[i], movWindow_len_Pose)));
             }
 
             //convert angles to 0 to 2pi range
             for(int i=0; i<3; i++) {
-                if(imu_Data.posePRY[i] > 2*RAD_TO_DEG(M_PI))
-                    imu_Data.posePRY[i] -= 2*RAD_TO_DEG(M_PI);
-                if(imu_Data.posePRY[i] < 0.0)
-                    imu_Data.posePRY[i] += 2*RAD_TO_DEG(M_PI);
+                if(Pose[i] > 2*RAD_TO_DEG(M_PI))
+                    Pose[i] -= 2*RAD_TO_DEG(M_PI);
+                if(Pose[i] < 0.0)
+                    Pose[i] += 2*RAD_TO_DEG(M_PI);
             }
 
             /** Todo: offset->filter quaternion, use this quaternion to generate gravity vector, and consequently aaReal and aaWorld  **/
@@ -390,16 +390,16 @@
 
     //moving window average of pose angle
     for(int i=0; i<3; i++) {
-        movWindow_Pose[i][movWindow_index_Pose] = sin(posePRY[i]);
-        Pose[i] = asin(generalFunctions::moving_window(movWindow_Pose[i], movWindow_len_Pose));
+        movWindow_Pose[i][movWindow_index_Pose] = sin(DEG_TO_RAD(posePRY[i]));
+        Pose[i] = RAD_TO_DEG(asin(generalFunctions::moving_window(movWindow_Pose[i], movWindow_len_Pose)));
     }
 
     //convert angles to 0 to 2pi range
     for(int i=0; i<3; i++) {
-        if(posePRY[i] > 2*RAD_TO_DEG(M_PI))
-            posePRY[i] -= 2*RAD_TO_DEG(M_PI);
-        if(posePRY[i] < 0.0)
-            posePRY[i] += 2*RAD_TO_DEG(M_PI);
+        if(Pose[i] > 2*RAD_TO_DEG(M_PI))
+            Pose[i] -= 2*RAD_TO_DEG(M_PI);
+        if(Pose[i] < 0.0)
+            Pose[i] += 2*RAD_TO_DEG(M_PI);
     }
 
     /** Todo: offset->filter quaternion, use this quaternion to generate gravity vector, and consequently aaReal and aaWorld  **/