Revision 13:02291aace2cf, committed 2016-08-07

Comitter:

kikoaac

Date:

Sun Aug 07 12:47:17 2016 +0000

Parent:

12:a70c193d8195

Commit message:

MPU;

Changed in this revision

--- a/MPU9250.cpp	Mon May 09 05:45:18 2016 +0000
+++ b/MPU9250.cpp	Sun Aug 07 12:47:17 2016 +0000
@@ -255,7 +255,7 @@
     for(i=0; i<3; i++) {
         bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
         data=(float)bit_data;
-        gyroscope_data[i]=data/gyro_divider;
+        gyroscope_data[i]=data;
     }
 
 }
@@ -553,13 +553,15 @@
     float R = sqrt(pow((float)accelerometer_data[0],2) + pow((float)accelerometer_data[1],2) + pow((float)accelerometer_data[2],2));
     angle_acc[0]=atan2(accelerometer_data[0],accelerometer_data[2])*(180 / 3.1415);
     angle_acc[1]=atan2(accelerometer_data[1],accelerometer_data[2])*(180 / 3.1415);
-    //angle_acc[2]=
+    angle_acc[2]=atan2(accelerometer_data[2],accelerometer_data[0])*(180 / 3.1415);
     /*angle_acc[1] =   -((180 / 3.1415) * acos((float)accelerometer_data[1] / R)-90);                                    // roll - angle of magnetic field vector in x direction
     angle_acc[0] =   (180 / 3.1415) * acos((float)accelerometer_data[0] / R)-90;                                    // pitch - angle of magnetic field vector in y direction
     angle_acc[2] =   (180 / 3.1415) * acos((float)accelerometer_data[2] / R); //*/
-    /*angle_acc[0] = atan2(accelerometer_data[0],sqrt(pow((float)accelerometer_data[1],2)+pow((float)accelerometer_data[2],2)))*180/3.1415;
+    /*
+    angle_acc[0] = atan2(accelerometer_data[0],sqrt(pow((float)accelerometer_data[1],2)+pow((float)accelerometer_data[2],2)))*180/3.1415;
     angle_acc[1] = atan2(accelerometer_data[1],sqrt(pow((float)accelerometer_data[0],2)+pow((float)accelerometer_data[2],2)))*180/3.1415;
-    angle_acc[2] = atan2(sqrt(pow((float)accelerometer_data[1],2)+pow((float)accelerometer_data[2],2)),accelerometer_data[2])*180/3.1415;*/
+    angle_acc[2] = atan2(sqrt(pow((float)accelerometer_data[1],2)+pow((float)accelerometer_data[2],2)),accelerometer_data[2])*180/3.1415;
+    */
     
 }
 void mpu9250_spi::get_rate()
@@ -569,7 +571,7 @@
     ReadRegs(MPUREG_GYRO_XOUT_H,response,6);
     for(int i=0; i<3; i++) {
         short bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
-        rate[i]=(double)(bit_data*0.01)-offset_t[i];
+        rate[i]=((double)(bit_data*0.01)-offset_t[i])/gyro_divider;;
         //printf("%d",rate[i]);
     }
 }
@@ -601,8 +603,9 @@
         //rate[i]=gyroscope_data[i]*0.01;
         double angA=angle_acc[i];
         S[i]  =((double)(rate[i]+prev_rate[i])*angleT.read()/2);
-        // S[i]=(floor(S[i]*100.0)/100.0);//-offset_angle[i];
-        angle[i]+=(floor(S[i]*10000.0)/10000.0);//-offset_angle[i];
+        S[i]=(floor(S[i]*100.0)/100.0);//-offset_angle[i];
+        if(i == 2) angle[i] += S[1]*gyro_divider;
+        else angle[i] += S[i]*gyro_divider;;//(floor(S[i]*10000.0)/10000.0);//-offset_angle[i];
         //angle[i]+=(double)S[i];
         Synthesis_angle[i]+=(double)angle[i];
         Synthesis_angle[i]=0.92*(double)(Synthesis_angle[i]+(double)rate[i])+0.08*angA;