Diff: zmu9250.h

Revision:

2:ce3ee4bc8cf7

Parent:

1:d8ce226c8c2e

--- a/zmu9250.h	Tue Dec 06 06:11:54 2016 +0000
+++ b/zmu9250.h	Tue Dec 06 12:19:49 2016 +0000
@@ -1,31 +1,28 @@
-#ifndef ZMU9250_H_
-#define ZMU9050_H_
-
-#endif
-
 #include "mbed.h"
 #include "MPU9250.h" 
 #include "math.h" 
 #include "kalman.h"
 
 Serial aa(USBTX,USBRX);
-class ZMU9250{   
-    
+
+
+class ZMU9250
+{
     public:
-        ZMU9250::ZMU9250()
+        ZMU9250()
         {
               
               //Set up I2C
-              //aa.printf("null\n");
               i2c.frequency(400000);  // use fast (400 kHz) I2C  
               this->t.start();        
               
               // Read the WHO_AM_I register, this is a good test of communication
+              uint8_t whoami = this->mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
               
-              uint8_t whoami = this->mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
-              aa.printf("whoami = %d\n\r ",whoami);
-              if ((whoami == 0x71))//||(whoami == 0x71)||(true)) // WHO_AM_I should always be 0x68
+              
+              if ((whoami == 0x71)||(whoami == 0x73)) // WHO_AM_I should always be 0x68
               {  
+              
                 wait(1);
                 this->mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
                 this->mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
@@ -34,18 +31,14 @@
                 this->mpu9250.initMPU9250(); 
                 this->mpu9250.initAK8963(magCalibration);
                 wait(1);
-                
                }
                else
                {
-                //aa.printf("forever\n");
                 while(1) ; // Loop forever if communication doesn't happen
                 }
-                //aa.printf("first\n");
                 this->mpu9250.getAres(); // Get accelerometer sensitivity
                 this->mpu9250.getGres(); // Get gyro sensitivity
                 this->mpu9250.getMres(); // Get magnetometer sensitivity
-                //aa.printf("second\n");
                 //magbias[0] = +470.;  // User environmental x-axis correction in milliGauss, should be automatically calculated
                 //magbias[1] = +120.;  // User environmental x-axis correction in milliGauss
                 //magbias[2] = +125.;  // User environmental x-axis correction in milliGauss
@@ -54,9 +47,8 @@
                 magbias[2] = +125;  // User environmental x-axis correction in milliGauss
         }
         
-        void ZMU9250::Update()
+        void Update()
         {
-           // aa.printf("hellow\n\r");
             if(this->mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {  // On interrupt, check if data ready interrupt
                 this->mpu9250.readAccelData(accelCount);  // Read the x/y/z adc values   
                 // Now we'll calculate the accleration value into actual g's
@@ -71,12 +63,10 @@
                 this->mpu9250.readMagData(magCount);  // Read the x/y/z adc values   
                 // Calculate the magnetometer values in milliGauss
                 // Include factory calibration per data sheet and user environmental corrections
-                //aa.printf("ten\n\r");
                 mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]+360.0f;  // get actual magnetometer value, this depends on scale being set
                 my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1]-210.0f;  
                 mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
-                //aa.printf("eleven\n\r");
-                //aa.printf("x %f\ty %f\tz %f\n\r",gyroCount[0],gy,gz);
+                //aa.printf("x %f\ty %f\tz %f\n",mx,my,mz);
                 
                 
             } // end if one
@@ -85,9 +75,9 @@
             lastUpdate = Now;
             this->sum += deltat;
             sumCount++;
+            this->mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  my,  mx, mz);
+            
             // Pass gyro rate as rad/s
-            this->mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
-            //this->mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  my,  mx, mz);
             /*if((rand()%20)>=0)
             {
             this->mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  my,  mx, mz);
@@ -115,7 +105,10 @@
                 yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);   
                 //yaw   = atan2(2.0f * (q[0] * q[2] + q[0] * q[3]), 1 - 2 * (  q[2] * q[2] + q[3] * q[3]));   
                 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
+                
+                //pitch = atan2(2.0f * (q[1] * q[3] - q[0] * q[2]),q[0]*q[0]-q[1]*q[1]+q[2]*q[2]-q[3]*q[3]);
                 roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
+                //pitch = atan2(sin(roll)*(q[1]*q[3]-q[0]*q[2]),q[1]*q[2]+q[0]*q[3]);
                 pitch *= 180.0f / PI;
                 yaw   *= 180.0f / PI; 
                 //yaw   -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
@@ -137,17 +130,17 @@
         }
         
         
-        float ZMU9250::Roll()
+        float Roll()
         {
           return roll_x;   
         }
         
-        float ZMU9250::Pitch()
+        float Pitch()
         {
           return pitch_y;   
         }
         
-        float ZMU9250::Yaw()
+        float Yaw()
         {
           return yaw_z;   
         }
@@ -168,4 +161,3 @@
 };
 
 
-