Diff: MPU9250.h

Revision:

12:a70c193d8195

Parent:

11:084e8ba240c1

--- a/MPU9250.h	Tue Jul 01 13:59:45 2014 +0000
+++ b/MPU9250.h	Mon May 09 05:45:18 2016 +0000
@@ -18,7 +18,7 @@
     unsigned int WriteReg( uint8_t WriteAddr, uint8_t WriteData );
     unsigned int ReadReg( uint8_t WriteAddr, uint8_t WriteData );
     void ReadRegs( uint8_t ReadAddr, uint8_t *ReadBuf, unsigned int Bytes );
-
+    void get_speedrate();
     bool init(int sample_rate_div,int low_pass_filter);
     void read_temp();
     void read_acc();
@@ -32,6 +32,9 @@
     unsigned int whoami();
     uint8_t  AK8963_whoami();
     void AK8963_read_Magnetometer();
+    void AK8963_setoffset(int x,double offset);
+    double *AK8936_read_Orientation(double *getdata);
+
     void read_all();
 
 
@@ -42,16 +45,111 @@
     int calib_data[3];
     float Magnetometer_ASA[3];
 
-    float accelerometer_data[3];
+    double accelerometer_data[3];
+    double accelerometer_data_prev[3];
     float Temperature;
     float gyroscope_data[3];
     float Magnetometer[3];
-    
+    float Magnetometer_offset[3];
+    double speed[3];
+    double meter[3];
+    float angle_acc[3];
+    short offset_gyro[3];
+    short rate[3];
+  
+    void set(float time=0.0001 , float time2=0.001){sampleTimeSpeed=time;sampleTimeMeter=time2;}       
+        double freefallSpeedSet(); double freefallSpeedGet(); 
+        double freefallMeterSet(); double freefallMeterGet();
+        
+        void SpeedSet();
+        void MeterSet();
+        
+        void Filter();
+        double VectolGet();double VectolSet();       
+        
+        void MPU_setup();
+        void MPU_setnum(int Num=500,float time=0.0001,double rate=0.00390635);//set data member
+        
+        void get_angle_acc();
+        void get_rate();
+        void set_angle();//set always [time]
+        void newset_angle(double ANG_x,double ANG_y,double ANG_z);
+        void get_angle(double *x,double *y,double *z);
+
+        void set_noise();
+        void set_offset();
+        void set_angleoffset();
+        short ratespeed[3];
+        short ratemeter[3];
+        double Synthesis_speed[3],kalman_speed[3],comp_speed[3];
+        double Synthesis_meter[3],kalman_meter[3],comp_meter[3];  
+        double angle[3],Synthesis_angle[3],kalman_angle[3],comp_angle[3];
   private:
     PinName _CS_pin;
     PinName _SO_pin;
     PinName _SCK_pin;
     float _error;
+    Timer angleT;
+    Timer speedT;
+    Timer meterT;
+
+        //ACC ADXL345
+        int x_acc,y_acc,z_acc,sampleNum;
+        double x_offset,y_offset,z_offset;
+        float gx,gy,gz,xnoise,ynoise,znoise;
+        //GYALO L3GD20
+        double Rate;
+        double  sampleTime;       
+        float noise[3];
+        short offset[3];short prev_rate[3];
+    
+        double t[3];
+        short tempDATA_ACC[3],tempDATA_ANGLE[3];
+        double offset_angle[3];
+
+    short prev_ratespeed[3],prev_speed[3];
+    short prev_ratemeter[3],prev_meter[3];
+    double real_acc[3];
+    short acc[3];
+    double filter_acc[3];
+      
+        int sampleTimeSpeed,sampleTimeMeter;
+        
+    class kalman
+        {
+            public:
+                kalman(void);
+                double getAngle(double newAngle, double newRate, double dt);
+                
+                void setAngle(double newAngle);
+                void setQangle(double newQ_angle);
+                void setQgyroBias(double newQ_gyroBias);
+                void setRangle(double newR_angle);
+                
+                double getRate(void);
+                double getQangle(void);
+                double getQbias(void);
+                double getRangle(void);
+                
+            
+            private:
+                double P[2][2];         //error covariance matrix
+                double K[2];            //kalman gain
+                double y;               //angle difference
+                double S;               //estimation error
+            
+                double rate;            //rate in deg/s
+                double angle;           //kalman angle
+                double bias;            //kalman gyro bias
+            
+                double Q_angle;         //process noise variance for the angle of the accelerometer
+                double Q_gyroBias;      //process noise variance for the gyroscope bias
+                double R_angle;         //measurement noise variance 
+        };   
+        kalman kalmaspeed[3];
+        kalman kalmameter[3];
+              //KALMAN
+        kalman kalma[3];
 };
 
 #endif