Revision 6:cc0a54642cdb, committed 2019-02-27

Comitter:

d15321854

Date:

Wed Feb 27 01:45:21 2019 +0000

Parent:

5:5839d1b118bc

Commit message:

Sensor Fusion

Changed in this revision

--- a/MPU9250.lib	Tue Jul 01 14:16:04 2014 +0000
+++ b/MPU9250.lib	Wed Feb 27 01:45:21 2019 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/kylongmu/code/MPU9250_SPI/#084e8ba240c1
+https://os.mbed.com/users/d15321854/code/MPU9250-XCLIN/#fafafd7e1805

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU9250_SPI_Test.lib	Wed Feb 27 01:45:21 2019 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/kylongmu/code/MPU9250_SPI_Test/#5839d1b118bc

--- a/main.cpp	Tue Jul 01 14:16:04 2014 +0000
+++ b/main.cpp	Wed Feb 27 01:45:21 2019 +0000
@@ -2,50 +2,412 @@
 kylongmu@msn.com
 */
 #include "mbed.h"
-#include "MPU9250.h"        //Include library
- 
+#include "MPU9250.h"
+#define Kp 0.5f         // proportional gain governs rate of convergence to accelerometer/magnetometer
+#define Ki 0.0f//0.005f       // integral gain governs rate of convergence of gyroscope biases
+ Timer LoopTimer;
+ Timer SensorTimer;
+float Times[10] = {0,0,0,0,0,0,0,0,0,0};
+float control_frequency = 800;//PPM_FREQU;         // frequency for the main loop in Hz
+int counter = 0;
+int divider = 20;
+float dt;                       // time for entire loop
+float dt_sensors;               // time only to read sensors
+float q0 = 1, q1 = 0, q2 = 0, q3 = 0;
+float q0_A = 1, q1_A = 0, q2_A = 0, q3_A = 0;
+float exInt = 0, eyInt = 0, ezInt = 0;
+float OX = 0, OY = 0, OZ = 0;
+float Mag_x_pre,Mag_y_pre,Mag_z_pre;
+float Mag_x_pre_L,Mag_y_pre_L,Mag_z_pre_L;
+float Mag_x_pre_LL,Mag_y_pre_LL,Mag_z_pre_LL;
+float Mag_x_pre_LLL,Mag_y_pre_LLL,Mag_z_pre_LLL;
+float Mag_x_ave,Mag_y_ave,Mag_x_total,Mag_y_total;
+float Cal_Mag_x_pre_LL,Cal_Mag_x_pre_L,Cal_Mag_x_pre,Cal_Mag_x;
+float GYRO_z_pre,GYRO_z_pre_L,GYRO_z_pre_LL,GYRO_z_pre_LLL;
+float GYRO_z_total,GYRO_z_offset,Global_GYRO_z;
+float Global_mag_vector_angle,Yaw_pre;
+float Global_mag_x_vector_angle,Mag_x_vector_angle;
+float Global_mag_y_vector_angle,Mag_y_vector_angle;
+int Count_mag_check=0;
+float angle[3];
+float Roll,Pitch,Yaw;
+float calibrated_values[3],magCalibrationp[3];
+float v_index[3];
+float dest1,dest2;
+int f=0;
+int j=0;
+int k=0;
+int g=0;
+int count1=0,count2=0,count3=0,count4=0,count5=0,count6=0,count7=0,count8=0,count9=0,count11=0,count12=0,count14=0;
+int Rot_index;
+float mRes = 10.*4912./32760.0;
 DigitalOut myled(LED1);
 Serial pc(SERIAL_TX, SERIAL_RX);
 SPI spi(SPI_MOSI, SPI_MISO, SPI_SCK);
 mpu9250_spi imu(spi,SPI_CS);   //define the mpu9250 object
+
+void Filter_IMUupdate(float halfT, float gx, float gy, float gz, float ax, float ay, float az) {
+    float norm;
+    float vx, vy, vz;
+    float ex, ey, ez;         
+    
+    // normalise the measurements
+    norm = sqrt(ax*ax + ay*ay + az*az);
+    if(norm == 0.0f) return;   
+    ax /= norm;
+    ay /= norm;
+    az /= norm;      
+    
+    // estimated direction of gravity
+    vx = 2*(q1*q3 - q0*q2);
+    vy = 2*(q0*q1 + q2*q3);
+    vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
+    
+    // error is sum of cross product between reference direction of field and direction measured by sensor
+    ex = (ay*vz - az*vy);
+    ey = (az*vx - ax*vz);
+    ez = (ax*vy - ay*vx);
+    
+    // integral error scaled integral gain
+    exInt += ex*Ki;
+    eyInt += ey*Ki;
+    ezInt += ez*Ki;
+    
+    // adjusted gyroscope measurements
+    gx += Kp*ex + exInt;
+    gy += Kp*ey + eyInt;
+    gz += Kp*ez + ezInt;
+    
+    // integrate quaternion rate and normalise
+    float q0o = q0; // he did the MATLAB to C error by not thinking of the beginning vector elements already being changed for the calculation of the rest!
+    float q1o = q1;
+    float q2o = q2;
+    float q3o = q3;
+    q0 += (-q1o*gx - q2o*gy - q3o*gz)*halfT;
+    q1 += (q0o*gx + q2o*gz - q3o*gy)*halfT;
+    q2 += (q0o*gy - q1o*gz + q3o*gx)*halfT;
+    q3 += (q0o*gz + q1o*gy - q2o*gx)*halfT;  
+    
+    // normalise quaternion
+    norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
+    q0 = q0 / norm;
+    q1 = q1 / norm;
+    q2 = q2 / norm;
+    q3 = q3 / norm;
+}
+void IMU_AHRSupdate(float halfT, float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
+    float norm;
+    float hx, hy, hz, bx, bz;
+    float vx, vy, vz, wx, wy, wz;
+    float ex, ey, ez;
+
+    // auxiliary variables to reduce number of repeated operations
+    float q0q0 = q0*q0;
+    float q0q1 = q0*q1;
+    float q0q2 = q0*q2;
+    float q0q3 = q0*q3;
+    float q1q1 = q1*q1;
+    float q1q2 = q1*q2;
+    float q1q3 = q1*q3;
+    float q2q2 = q2*q2;   
+    float q2q3 = q2*q3;
+    float q3q3 = q3*q3;          
+    
+    // normalise the measurements
+    norm = sqrt(ax*ax + ay*ay + az*az);
+    if(norm == 0.0f) return;
+    ax = ax / norm;
+    ay = ay / norm;
+    az = az / norm;
+    norm = sqrt(mx*mx + my*my + mz*mz);
+  
+    if(norm == 0.0f) return;
+    mx = mx / norm;
+    my = my / norm;
+    mz = mz / norm;         
+    /*OX=mx;
+    OY=my;
+    OZ=mz;*/
+    // compute reference direction of flux
+    hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
+    hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
+    hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);         
+    bx = sqrt((hx*hx) + (hy*hy));
+    bz = hz;        
+    // estimated direction of gravity and flux (v and w)
+    vx = 2*(q1q3 - q0q2);
+    vy = 2*(q0q1 + q2q3);
+    vz = q0q0 - q1q1 - q2q2 + q3q3;
+    wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
+    wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
+    wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);  
+    
+    // error is sum of cross product between reference direction of fields and direction measured by sensors
+    ex = (ay*vz - az*vy) + (my*wz - mz*wy);
+    ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
+    ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
+    
+
+    // integral error scaled integral gain
+    exInt = exInt + ex*Ki;
+    eyInt = eyInt + ey*Ki;
+    ezInt = ezInt + ez*Ki;
+  
+    // adjusted gyroscope measurements
+    gx = gx + Kp*ex + exInt;
+    gy = gy + Kp*ey + eyInt;
+    gz = gz + Kp*ez + ezInt;
+    
+    // integrate quaternion rate and normalise
+    q0_A = q0_A + (-q1*gx - q2*gy - q3*gz)*halfT;
+    q1_A = q1_A + (q0*gx + q2*gz - q3*gy)*halfT;
+    q2_A = q2_A + (q0*gy - q1*gz + q3*gx)*halfT;
+    q3_A = q3_A + (q0*gz + q1*gy - q2*gx)*halfT;  
+    
+    // normalise quaternion
+    norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
+    q0_A = q0_A / norm;
+    q1_A = q1_A / norm;
+    q2_A = q2_A / norm;
+    q3_A = q3_A / norm;
+}
+
+    
+    
+
+void Filter_compute(float dt, const float * Gyro_data, const float * Acc_data, const float * Comp_data)
+{
+    // IMU/AHRS
+
+    float d_Gyro_angle[3];
+    void get_Acc_angle(const float * Acc_data);
+     // IMU/AHRS (from http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/)   
+    float radGyro[3],Gyro_cal_data; // Gyro in radians per second
+    
+    for(int i=0; i<3; i++)
+        radGyro[i] = Gyro_data[i] * 3.14159/ 180;
+        
+        Filter_IMUupdate(dt/2, radGyro[0], radGyro[1], radGyro[2], Acc_data[0], Acc_data[1], Acc_data[2]);
+        IMU_AHRSupdate(dt/2, radGyro[0], radGyro[1], radGyro[2], Acc_data[0], Acc_data[1], Acc_data[2], Comp_data[0], Comp_data[1], Comp_data[2]);
+        
+        float rangle[3]; // calculate angles in radians from quternion output, formula from Wiki (http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles)
+        
+        rangle[0] = atan2(2*q0*q1 + 2*q2*q3, 1 - 2*(q1*q1 + q2*q2));
+        rangle[1] = asin (2*q0*q2 - 2*q3*q1);
+        rangle[2] = atan2(2*q0_A*q3_A + 2*q1_A*q2_A, 1 - 2*(q2_A*q2_A + q3_A*q3_A));            // Z-axis
+        
+        
+        for(int i=0; i<2; i++){  // angle in degree
+            angle[i] = rangle[i] * 180 / 3.14159;
+        }
+        Roll=angle[0];
+        Pitch=angle[1];
+//**************************************************Gyro_data[2]  filter      start
+        float GYRO_z=0;
+
+        GYRO_z=Gyro_data[2]*0.15 + GYRO_z_pre*0.20 + GYRO_z_pre_L*0.20 + GYRO_z_pre_LL*0.25 + GYRO_z_pre_LLL*0.20;
+        if( count4==1 ){
+            GYRO_z_pre_L=GYRO_z_pre;
+            
+            count4=0;
+        }
+        if( count5==2 ){
+            GYRO_z_pre_LL=GYRO_z_pre_L;
+            
+            count5=0;
+        }
+        if( count6==3 ){
+            GYRO_z_pre_LLL=GYRO_z_pre_LL;
+            
+            count6=0;
+        }
+        
+       
+        
+        count4++;
+        count5++;
+        count6++;
+        GYRO_z_pre=Gyro_data[2];
+        Global_GYRO_z=GYRO_z;
+       /*pc.printf("     GYRO_z:%10.3f     ,count8:%10d   ", 
+            GYRO_z,
+            count8
+            );*/
+        if((count8>5)&&(count8<=805)){
+            GYRO_z_total+=GYRO_z;
+        }
+        if( count8==805 ){
+            GYRO_z_offset=GYRO_z_total/800;
+            /*pc.printf("     GYRO_z_offset:%10.5f     ", 
+            GYRO_z_offset
+            );*/
+            GYRO_z_total=0;
+            count8=0;
+        }
+
+        count8++;
+//**************************************************Gyro_data[2]'s average  filter   :     answer=GYRO_Z is roughly = 1.05447
+
+//************************************************** calculate Yaw
+    if( (count11==35) ){
+        if( abs(Yaw_pre-Yaw)<1 ){
+            Yaw_pre=Yaw_pre;
+        }else{
+            Yaw_pre=Yaw;
+        }
+        count11=0;
+    }
+    count11++;
+        
+    if( count12>=20 ){
+        Yaw += (Gyro_data[2]-1.05447) *dt; 
+    }
+    count12++;
+    pc.printf("     Yaw:%10.5f     ", 
+            Yaw
+    );
+}
+void Mag_Complentary_Filter(float dt, const float * Comp_data)
+{
+        float Mag_x=0,Mag_y=0,Mag_z=0;
+        Mag_x=Comp_data[0]*0.15 + Mag_x_pre*0.20 + Mag_x_pre_L*0.20 + Mag_x_pre_LL*0.25 + Mag_x_pre_LLL*0.20;
+        Mag_y=Comp_data[1]*0.15 + Mag_y_pre*0.20 + Mag_y_pre_L*0.20 + Mag_y_pre_LL*0.25 + Mag_y_pre_LLL*0.20;
+        Mag_z=Comp_data[2]*0.15 + Mag_z_pre*0.20 + Mag_z_pre_L*0.20 + Mag_z_pre_LL*0.25 + Mag_z_pre_LLL*0.20;
+        
+        
+        if( count1==1 ){
+            Mag_x_pre_L=Mag_x_pre;
+            Mag_y_pre_L=Mag_y_pre;
+            Mag_z_pre_L=Mag_z_pre;
+            Cal_Mag_x_pre=Cal_Mag_x;
+            
+            count1=0;
+        }
+        if( count2==2 ){
+            Mag_x_pre_LL=Mag_x_pre_L;
+            Mag_y_pre_LL=Mag_y_pre_L;
+            Mag_z_pre_LL=Mag_z_pre_L;
+            Cal_Mag_x_pre_L=Cal_Mag_x_pre;
+            
+            count2=0;
+        }
+        if( count7==3 ){
+            Mag_x_pre_LLL=Mag_x_pre_LL;
+            Mag_y_pre_LLL=Mag_y_pre_LL;
+            Mag_z_pre_LLL=Mag_z_pre_LL;
+            Cal_Mag_x_pre_LL=Cal_Mag_x_pre_L;
+        
+            count7=0;
+        }
+
+        
+        count1++;
+        count2++;
+        count7++;
+        Mag_x_pre=Comp_data[0];
+        Mag_y_pre=Comp_data[1];
+        Mag_z_pre=Comp_data[2];
+        if( count14>4 ){ 
+            Cal_Mag_x=Mag_x;             
+        }
+        count14++;
+        
+        
+//*************************************Mag_ave calculate
+        if(count3<=20){
+            Mag_x_total+=Mag_x;
+            Mag_y_total+=Mag_y;
+        }
+        if( count3==20){
+            Mag_x_ave=Mag_x_total/21;
+            Mag_y_ave=Mag_y_total/21;
+            /*pc.printf("     Mag_x_ave:%10.5f    ,Mag_y_ave:%10.5f     ", 
+            Mag_x_ave,
+            Mag_y_ave
+            );*/
+            Mag_x_total=0;
+            Mag_y_total=0;
+            count3=0;
+            
+            
+        }
+        count3++;
+        
+//********************************ROT_check  start
+
+        float v_length,v_length_ave,MagVector_angle;
+        v_length=sqrt( Mag_x*Mag_x + Mag_y*Mag_y );
+        v_length_ave=sqrt( Mag_x_ave*Mag_x_ave + Mag_y_ave*Mag_y_ave );
+        
+        MagVector_angle=acos(( Mag_x*Mag_x_ave + Mag_y*Mag_y_ave )/(v_length*v_length_ave))*57.3;
+        
+        if( count9==3 ){
+            Global_mag_vector_angle=MagVector_angle;
+            count9=0;
+        }
+        count9++;
+        
+        
+        if( (abs(Global_mag_vector_angle-MagVector_angle)<5) && (abs(Global_GYRO_z)<5) ){
+            Count_mag_check++;
+            
+        }else{ Count_mag_check=0; }
+        
+        if( Count_mag_check==30 ){
+            Yaw=Yaw_pre;
+            Count_mag_check=0;
+        }
+        float ABS_CHECK=abs(Global_mag_vector_angle-MagVector_angle);
+//********************************Theta_check  end
+        /*pc.printf("ABS_CHECK:%10.3f,Cal_Mag_x_pre_LL:%10.3f,Mag_x:%10.3f,Count_mag_check:%10d ,Yaw_pre:%10.3f,Yaw_filter:%10.3f    ", 
+            ABS_CHECK,
+            Cal_Mag_x_pre_LL,
+            Mag_x,
+            Count_mag_check,
+            Yaw_pre,
+            Yaw
+            );*/
+
+}
 int main(){
     pc.baud(115200);
     if(imu.init(1,BITS_DLPF_CFG_188HZ)){  //INIT the mpu9250
-        printf("\nCouldn't initialize MPU9250 via SPI!");
+        pc.printf("\nCouldn't initialize MPU9250 via SPI!");
     }    
-    printf("\nWHOAMI=0x%2x\n",imu.whoami()); //output the I2C address to know if SPI is working, it should be 104
+    pc.printf("\nWHOAMI=0x%2x\n",imu.whoami()); //output the I2C address to know if SPI is working, it should be 104
     wait(1);    
-    printf("Gyro_scale=%u\n",imu.set_gyro_scale(BITS_FS_2000DPS));    //Set full scale range for gyros
+    pc.printf("Gyro_scale=%u\n",imu.set_gyro_scale(BITS_FS_2000DPS));    //Set full scale range for gyros
     wait(1);  
-    printf("Acc_scale=%u\n",imu.set_acc_scale(BITS_FS_16G));          //Set full scale range for accs
+    pc.printf("Acc_scale=%u\n",imu.set_acc_scale(BITS_FS_16G));          //Set full scale range for accs
     wait(1);
-    printf("AK8963 WHIAM=0x%2x\n",imu.AK8963_whoami());
+    pc.printf("AK8963 WHIAM=0x%2x\n",imu.AK8963_whoami());
     wait(0.1);  
     imu.AK8963_calib_Magnetometer();
     while(1) {
-        //myled = 1;
+
+        LoopTimer.start();
         wait(0.1);
-        /*
-        imu.read_temp();
-        imu.read_acc();
-        imu.read_rot();
-        imu.AK8963_read_Magnetometer();
-        */
+        
+        SensorTimer.start();
+        Times[1] = LoopTimer.read(); // 197us
+        SensorTimer.stop(); // stop time for measuring sensors
+        dt_sensors = SensorTimer.read();
+        SensorTimer.reset();
+        // meassure dt since last measurement for the filter
+        dt = LoopTimer.read(); // time in s since last loop
+        LoopTimer.reset();
+        
+        
         imu.read_all();
-        printf("%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f\n", 
-            imu.Temperature,
-            imu.gyroscope_data[0],
-            imu.gyroscope_data[1],
-            imu.gyroscope_data[2],
-            imu.accelerometer_data[0],
-            imu.accelerometer_data[1],
-            imu.accelerometer_data[2],
-            imu.Magnetometer[0],
-            imu.Magnetometer[1],
-            imu.Magnetometer[2]
+        
+        Mag_Complentary_Filter(dt,imu.Magnetometer);
+        Filter_compute(dt, imu.gyroscope_data, imu.accelerometer_data, imu.Magnetometer);
+        
+        pc.printf("Roll:%10.3f,Pitch:%10.3f,Yaw:%10.3f   \n", 
+            Roll,
+            Pitch,
+            Yaw
             );
-        //myled = 0;
-        //wait(0.5);
+        
     }
 }
- 
\ No newline at end of file

--- a/mbed.bld	Tue Jul 01 14:16:04 2014 +0000
+++ b/mbed.bld	Wed Feb 27 01:45:21 2019 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_official/code/mbed/builds/024bf7f99721
\ No newline at end of file
+http://mbed.org/users/mbed_official/code/mbed/builds/6c34061e7c34
\ No newline at end of file