Diff: zmu9250.h

Revision:

1:d8ce226c8c2e

Parent:

0:a291977ec0b1

Child:

2:ce3ee4bc8cf7

--- a/zmu9250.h	Mon Dec 05 18:31:43 2016 +0000
+++ b/zmu9250.h	Tue Dec 06 06:11:54 2016 +0000
@@ -1,24 +1,30 @@
+#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");
+              //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
-              if (whoami == 0x73) // WHO_AM_I should always be 0x68
+              aa.printf("whoami = %d\n\r ",whoami);
+              if ((whoami == 0x71))//||(whoami == 0x71)||(true)) // WHO_AM_I should always be 0x68
               {  
                 wait(1);
                 this->mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
@@ -32,14 +38,14 @@
                }
                else
                {
-                aa.printf("forever\n");
+                //aa.printf("forever\n");
                 while(1) ; // Loop forever if communication doesn't happen
                 }
-                aa.printf("first\n");
+                //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");
+                //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
@@ -48,8 +54,9 @@
                 magbias[2] = +125;  // User environmental x-axis correction in milliGauss
         }
         
-        void Update()
+        void ZMU9250::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
@@ -64,12 +71,12 @@
                 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");
+                //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",mx,my,mz);
+                //aa.printf("eleven\n\r");
+                //aa.printf("x %f\ty %f\tz %f\n\r",gyroCount[0],gy,gz);
                 
                 
             } // end if one
@@ -79,14 +86,16 @@
             this->sum += deltat;
             sumCount++;
             // Pass gyro rate as rad/s
-            if((rand()%20)>=0)
+            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);
             }else
             {
             //this->mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
             this->mpu9250.Mad_Update(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
-            }
+            }*/
             
             
             // Serial print and/or display at 0.5 s rate independent of data rates
@@ -128,17 +137,17 @@
         }
         
         
-        float Roll()
+        float ZMU9250::Roll()
         {
           return roll_x;   
         }
         
-        float Pitch()
+        float ZMU9250::Pitch()
         {
           return pitch_y;   
         }
         
-        float Yaw()
+        float ZMU9250::Yaw()
         {
           return yaw_z;   
         }
@@ -149,6 +158,7 @@
         uint32_t sumCount;
         char buffer[14];
         int roll_x;
+        kalman kal();
         int pitch_y;
         int yaw_z;
         MPU9250 mpu9250;
@@ -158,3 +168,4 @@
 };
 
 
+