Diff: imuHandler.cpp

Revision:

6:70818c388698

Parent:

5:42b4f2fb593f

Child:

7:132bd7a919f5

--- a/imuHandler.cpp	Mon May 16 07:40:52 2016 +0000
+++ b/imuHandler.cpp	Mon May 16 08:00:42 2016 +0000
@@ -1,13 +1,13 @@
- /*
- * Copyright (C) 2016 Akash Vibhute <akash.roboticist@gmail.com>
- * 
- * Wrapper to handle all quirks of MPU9250 and BNO055 IMU on Virgo 3 robot. 
- * Based off various user libraries
- *
- *
- * Initial Release: Apr/26/2016
- *
- */
+/*
+* Copyright (C) 2016 Akash Vibhute <akash.roboticist@gmail.com>
+*
+* Wrapper to handle all quirks of MPU9250 and BNO055 IMU on Virgo 3 robot.
+* Based off various user libraries
+*
+*
+* Initial Release: Apr/26/2016
+*
+*/
 
 #include "imuHandler.h"
 
@@ -19,7 +19,7 @@
     unsigned int movWindowSize_GyroAcc = GyroAcc_MWindowSize;
 
     unstable_readings = MPU6050_StabilizationReadings;
-        
+
     imu_stabilized=false;
 
     if(movWindowSize_Pose <= movWindow_lenMax) movWindow_len_Pose = movWindowSize_Pose;
@@ -136,14 +136,25 @@
 
             //convert angles to 0 to 2pi range
             for(int i=0; i<3; i++) {
-                if(imu_Data.posePRY[i] > 2*M_PI)  imu_Data.posePRY[i] -= 2*M_PI;
-                if(imu_Data.posePRY[i] < 0)       imu_Data.posePRY[i] += 2*M_PI;
+                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);
             }
 
+
             //moving window average of pose angle
             for(int i=0; i<3; i++) {
-                movWindow_Pose[i][movWindow_index_Pose] = RAD_TO_DEG(imu_Data.posePRY[i]);
-                Pose[i] = generalFunctions::moving_window(movWindow_Pose[i]/**/, movWindow_len_Pose);    /****** PROBABLY WILL BREAK HERE!!! ******/
+                movWindow_Pose[i][movWindow_index_Pose] = sin(posePRY[i]);
+                Pose[i] = 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);
             }
 
             /** Todo: offset->filter quaternion, use this quaternion to generate gravity vector, and consequently aaReal and aaWorld  **/
@@ -201,8 +212,8 @@
 
 void IMU_MPU6050::getPose(float *pose[3])
 {
-    //pitch about X, roll about Y, yaw about Z 
-    for(int i=0; i<3; i++)      
+    //pitch about X, roll about Y, yaw about Z
+    for(int i=0; i<3; i++)
         *pose[i]=Pose[i];
 }
 
@@ -233,8 +244,8 @@
 {
     IMU_MPU6050::imuUpdate(); //refresh imu before updating Pose
 
-    //pitch about X, roll about Y, yaw about Z   
-    for(int i=0; i<3; i++)   
+    //pitch about X, roll about Y, yaw about Z
+    for(int i=0; i<3; i++)
         PoseCorrection[i]=(Pose[i] - pose_in[i]);
 }
 
@@ -263,8 +274,8 @@
 
     for(int i=0; i<3; i++)
         PoseCorrection[i]=0;
-    
-    bno_timer.start();    
+
+    bno_timer.start();
 }
 
 bool IMU_BNO055::imuInit()
@@ -272,7 +283,7 @@
     imu_BNO055.reset();
 
     if (imu_BNO055.check()) {
-        
+
         //load default calib parameters
         offset_acc[0] =10;
         offset_acc[1] =509;
@@ -338,7 +349,7 @@
     if(imu_stabilized[1] == 0) { //for imu only mode
         if(imu_stabilized[0] == 0)
             imu_stabilized[0] = 1;
-            
+
         if( ((generalFunctions::abs_f(imu_BNO055.lia.x) - initialAcc[0])* 9.81*1000 <= 5.0) && ((generalFunctions::abs_f(imu_BNO055.lia.y) - initialAcc[1])* 9.81*1000 <= 5.0) && ((generalFunctions::abs_f(imu_BNO055.lia.z) - initialAcc[2])* 9.81*1000 <= 5.0)) {
 
             unstable_readings--;
@@ -369,7 +380,6 @@
     posePRY[2] = (2*RAD_TO_DEG(M_PI) - imu_BNO055.euler.yaw) - imu_initialAngles[2] - PoseCorrection[2];
 
     //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);
@@ -380,8 +390,16 @@
 
     //moving window average of pose angle
     for(int i=0; i<3; i++) {
-        movWindow_Pose[i][movWindow_index_Pose] = posePRY[i];
-        Pose[i] = generalFunctions::moving_window(movWindow_Pose[i], movWindow_len_Pose);
+        movWindow_Pose[i][movWindow_index_Pose] = sin(posePRY[i]);
+        Pose[i] = 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);
     }
 
     /** Todo: offset->filter quaternion, use this quaternion to generate gravity vector, and consequently aaReal and aaWorld  **/
@@ -416,8 +434,8 @@
 
 void IMU_BNO055::getPose(float *pose[3])
 {
-    //pitch about X, roll about Y, yaw about Z 
-    for(int i=0; i<3; i++)      
+    //pitch about X, roll about Y, yaw about Z
+    for(int i=0; i<3; i++)
         *pose[i]=Pose[i];
 }
 
@@ -447,9 +465,9 @@
 void IMU_BNO055::setPose(float pose_in[3])
 {
     IMU_BNO055::imuUpdate(); //refresh imu before updating Pose
-    
+
     //pitch about X, roll about Y, yaw about Z
-    for(int i=0; i<3; i++)   
+    for(int i=0; i<3; i++)
         PoseCorrection[i]=(Pose[i] - pose_in[i]);
 }
 
@@ -457,24 +475,24 @@
 {
     //imu_BNO055.get_calib();
     imu_BNO055.read_calibration_data();
-    
+
     for(int i=0; i<22; i++)
         calibration_regs[i]=imu_BNO055.calibration[i];
-        
+
     offset_acc[0] = (calibration_regs[1] + calibration_regs[0]);
     offset_acc[1] = (calibration_regs[3] + calibration_regs[2]);
     offset_acc[2] = (calibration_regs[5] + calibration_regs[4]);
-    
+
     offset_mag[0] = (calibration_regs[7] + calibration_regs[6]);
     offset_mag[1] = (calibration_regs[9] + calibration_regs[8]);
     offset_mag[2] = (calibration_regs[11] + calibration_regs[10]);
-    
+
     offset_gyr[0] = (calibration_regs[13] + calibration_regs[12]);
     offset_gyr[1] = (calibration_regs[15] + calibration_regs[14]);
     offset_gyr[2] = (calibration_regs[17] + calibration_regs[16]);
-    
+
     radius_acc = (calibration_regs[19] + calibration_regs[18]);
-    radius_mag = (calibration_regs[21] + calibration_regs[20]);    
+    radius_mag = (calibration_regs[21] + calibration_regs[20]);
 }
 
 void IMU_BNO055::writeCalibrationData()
@@ -482,44 +500,44 @@
     //acc
     calibration_regs[0] = offset_acc[0] & 0b00001111;
     calibration_regs[1] = offset_acc[0] & 0b11110000;
-    
+
     calibration_regs[2] = offset_acc[1] & 0b00001111;
     calibration_regs[3] = offset_acc[1] & 0b11110000;
-    
+
     calibration_regs[4] = offset_acc[2] & 0b00001111;
     calibration_regs[5] = offset_acc[2] & 0b11110000;
-    
+
     //mag
     calibration_regs[6] = offset_mag[0] & 0b00001111;
     calibration_regs[7] = offset_mag[0] & 0b11110000;
-    
+
     calibration_regs[8] = offset_mag[1] & 0b00001111;
     calibration_regs[9] = offset_mag[1] & 0b11110000;
-    
+
     calibration_regs[10] = offset_mag[2] & 0b00001111;
     calibration_regs[11] = offset_mag[2] & 0b11110000;
-    
+
     //gyro
     calibration_regs[12] = offset_gyr[0] & 0b00001111;
     calibration_regs[13] = offset_gyr[0] & 0b11110000;
-    
+
     calibration_regs[14] = offset_gyr[1] & 0b00001111;
     calibration_regs[15] = offset_gyr[1] & 0b11110000;
-    
+
     calibration_regs[16] = offset_gyr[2] & 0b00001111;
     calibration_regs[17] = offset_gyr[2] & 0b11110000;
-    
+
     //acc_radius
     calibration_regs[18] = radius_acc & 0b00001111;
     calibration_regs[19] = radius_acc & 0b11110000;
-    
+
     //mag_radius
     calibration_regs[20] = radius_mag & 0b00001111;
     calibration_regs[21] = radius_mag & 0b11110000;
-    
+
     for(int i=0; i<22; i++)
         imu_BNO055.calibration[i] = calibration_regs[i];
-    
+
     imu_BNO055.write_calibration_data();
 }