Diff: imuHandler.cpp

Revision:

10:31eee5f90d04

Parent:

9:47b6a8530868

Child:

11:fb47aa30cc7b

Child:

12:6ec427e47641

--- a/imuHandler.cpp	Tue Jun 07 03:48:09 2016 +0000
+++ b/imuHandler.cpp	Thu Jun 16 03:27:34 2016 +0000
@@ -17,19 +17,27 @@
 {
     unsigned int movWindowSize_Pose = Pose_MWindowSize;
     unsigned int movWindowSize_GyroAcc = GyroAcc_MWindowSize;
-
+    unsigned int movWindowSize_Mag = Mag_MWindowSize;
+    
     unstable_readings = MPU6050_StabilizationReadings;
-        
-    imu_stabilized=false;
+
+    imu_stabilized[0]=0;
 
     if(movWindowSize_Pose <= movWindow_lenMax) movWindow_len_Pose = movWindowSize_Pose;
     else movWindow_len_Pose = movWindow_lenMax;
 
     if(movWindow_len_GyroAcc <= movWindow_lenMax) movWindow_len_GyroAcc = movWindowSize_GyroAcc;
     else movWindow_len_GyroAcc = movWindow_lenMax;
+    
+    if(movWindowSize_Mag <= movWindow_lenMax) movWindow_len_Mag = movWindowSize_Mag;
+    else movWindow_len_Mag = movWindow_lenMax;
 
     movWindow_index_Pose=0;
     movWindow_index_GyroAcc=0;
+    movWindow_index_Mag=0;
+    
+    enable_mag =0; //disable magnetometer reading by default
+    mag_time =0;
 
     for(int i=0; i<3; i++)
         PoseCorrection[i]=0;
@@ -76,8 +84,8 @@
     // Check that this interrupt is a FIFO buffer overflow interrupt.
     if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
 
-        /*imu_mpu6050.resetFIFO();
-        debug_printf("FIFO overflow!\n");*/
+        imu_mpu6050.resetFIFO();
+        /*debug_printf("FIFO overflow!\n");*/
         return;
 
         // Check that this interrupt is a Data Ready interrupt.
@@ -106,7 +114,7 @@
                     mpu_timer.start();
                     unstable_readings --;
 
-                    imu_stabilized = true;
+                    imu_stabilized[0] = 1;
                     //debug_printf("\nIMU stabilized\n");
                 }
             }
@@ -119,6 +127,25 @@
             imu_mpu6050.dmpGetQuaternion(&imu_Data.q, fifoBuffer);
             imu_mpu6050.dmpGetGravity(&imu_Data.gravity, &imu_Data.q);
             imu_mpu6050.dmpGetYawPitchRoll(yawPitchRoll, &imu_Data.q, &imu_Data.gravity);
+            
+            if((enable_mag == 1) && ((time_s - mag_time)*1000 >= mpu_mag_readms) ){
+                int16_t mx,my,mz;
+                
+                imu_mpu6050.getMag(&mx, &my, &mz);
+                mag_time = time_s;
+                
+                imu_Data.magnetometer[0]  = (float)mx /10.0;
+                imu_Data.magnetometer[1]  = (float)my /10.0;
+                imu_Data.magnetometer[2]  = (float)mz /10.0;
+
+                //moving window average of pose angle
+                for(int i=0; i<3; i++) {
+                    movWindow_Mag[i][movWindow_index_Mag] = imu_Data.magnetometer[i];
+                    Mag[i] = generalFunctions::moving_window(movWindow_Mag[i], movWindow_len_Mag);
+                }
+
+            }
+
 
             //debug_printf("DEBUG>> got YPR data\n");
 
@@ -136,14 +163,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(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);
             }
 
+
             //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(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(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  **/
@@ -201,8 +239,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 +271,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]);
 }