Samuel Lin / Mbed 2 deprecated MPU9250_SPI_Test_1201

College Project

Dependencies:   mbed MPU9250_SPI_Test MPU9250-XCLIN

main.cpp@6:cc0a54642cdb, 2019-02-27 (annotated)

Committer:
d15321854
Date:
Wed Feb 27 01:45:21 2019 +0000
Revision:
6:cc0a54642cdb
Parent:
5:5839d1b118bc 
Sensor Fusion

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kylongmu 3:ad2ef67e7f72 1 /*CODED by Qiyong Mu on 21/06/2014
kylongmu 3:ad2ef67e7f72 2 kylongmu@msn.com
kylongmu 3:ad2ef67e7f72 3 */
kylongmu 0:58f9d4556df7 4 #include "mbed.h"
d15321854 6:cc0a54642cdb 5 #include "MPU9250.h"
d15321854 6:cc0a54642cdb 6 #define Kp 0.5f // proportional gain governs rate of convergence to accelerometer/magnetometer
d15321854 6:cc0a54642cdb 7 #define Ki 0.0f//0.005f // integral gain governs rate of convergence of gyroscope biases
d15321854 6:cc0a54642cdb 8 Timer LoopTimer;
d15321854 6:cc0a54642cdb 9 Timer SensorTimer;
d15321854 6:cc0a54642cdb 10 float Times[10] = {0,0,0,0,0,0,0,0,0,0};
d15321854 6:cc0a54642cdb 11 float control_frequency = 800;//PPM_FREQU; // frequency for the main loop in Hz
d15321854 6:cc0a54642cdb 12 int counter = 0;
d15321854 6:cc0a54642cdb 13 int divider = 20;
d15321854 6:cc0a54642cdb 14 float dt; // time for entire loop
d15321854 6:cc0a54642cdb 15 float dt_sensors; // time only to read sensors
d15321854 6:cc0a54642cdb 16 float q0 = 1, q1 = 0, q2 = 0, q3 = 0;
d15321854 6:cc0a54642cdb 17 float q0_A = 1, q1_A = 0, q2_A = 0, q3_A = 0;
d15321854 6:cc0a54642cdb 18 float exInt = 0, eyInt = 0, ezInt = 0;
d15321854 6:cc0a54642cdb 19 float OX = 0, OY = 0, OZ = 0;
d15321854 6:cc0a54642cdb 20 float Mag_x_pre,Mag_y_pre,Mag_z_pre;
d15321854 6:cc0a54642cdb 21 float Mag_x_pre_L,Mag_y_pre_L,Mag_z_pre_L;
d15321854 6:cc0a54642cdb 22 float Mag_x_pre_LL,Mag_y_pre_LL,Mag_z_pre_LL;
d15321854 6:cc0a54642cdb 23 float Mag_x_pre_LLL,Mag_y_pre_LLL,Mag_z_pre_LLL;
d15321854 6:cc0a54642cdb 24 float Mag_x_ave,Mag_y_ave,Mag_x_total,Mag_y_total;
d15321854 6:cc0a54642cdb 25 float Cal_Mag_x_pre_LL,Cal_Mag_x_pre_L,Cal_Mag_x_pre,Cal_Mag_x;
d15321854 6:cc0a54642cdb 26 float GYRO_z_pre,GYRO_z_pre_L,GYRO_z_pre_LL,GYRO_z_pre_LLL;
d15321854 6:cc0a54642cdb 27 float GYRO_z_total,GYRO_z_offset,Global_GYRO_z;
d15321854 6:cc0a54642cdb 28 float Global_mag_vector_angle,Yaw_pre;
d15321854 6:cc0a54642cdb 29 float Global_mag_x_vector_angle,Mag_x_vector_angle;
d15321854 6:cc0a54642cdb 30 float Global_mag_y_vector_angle,Mag_y_vector_angle;
d15321854 6:cc0a54642cdb 31 int Count_mag_check=0;
d15321854 6:cc0a54642cdb 32 float angle[3];
d15321854 6:cc0a54642cdb 33 float Roll,Pitch,Yaw;
d15321854 6:cc0a54642cdb 34 float calibrated_values[3],magCalibrationp[3];
d15321854 6:cc0a54642cdb 35 float v_index[3];
d15321854 6:cc0a54642cdb 36 float dest1,dest2;
d15321854 6:cc0a54642cdb 37 int f=0;
d15321854 6:cc0a54642cdb 38 int j=0;
d15321854 6:cc0a54642cdb 39 int k=0;
d15321854 6:cc0a54642cdb 40 int g=0;
d15321854 6:cc0a54642cdb 41 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;
d15321854 6:cc0a54642cdb 42 int Rot_index;
d15321854 6:cc0a54642cdb 43 float mRes = 10.*4912./32760.0;
kylongmu 0:58f9d4556df7 44 DigitalOut myled(LED1);
kylongmu 0:58f9d4556df7 45 Serial pc(SERIAL_TX, SERIAL_RX);
kylongmu 0:58f9d4556df7 46 SPI spi(SPI_MOSI, SPI_MISO, SPI_SCK);
kylongmu 0:58f9d4556df7 47 mpu9250_spi imu(spi,SPI_CS); //define the mpu9250 object
d15321854 6:cc0a54642cdb 48
d15321854 6:cc0a54642cdb 49 void Filter_IMUupdate(float halfT, float gx, float gy, float gz, float ax, float ay, float az) {
d15321854 6:cc0a54642cdb 50 float norm;
d15321854 6:cc0a54642cdb 51 float vx, vy, vz;
d15321854 6:cc0a54642cdb 52 float ex, ey, ez;
d15321854 6:cc0a54642cdb 53
d15321854 6:cc0a54642cdb 54 // normalise the measurements
d15321854 6:cc0a54642cdb 55 norm = sqrt(ax*ax + ay*ay + az*az);
d15321854 6:cc0a54642cdb 56 if(norm == 0.0f) return;
d15321854 6:cc0a54642cdb 57 ax /= norm;
d15321854 6:cc0a54642cdb 58 ay /= norm;
d15321854 6:cc0a54642cdb 59 az /= norm;
d15321854 6:cc0a54642cdb 60
d15321854 6:cc0a54642cdb 61 // estimated direction of gravity
d15321854 6:cc0a54642cdb 62 vx = 2*(q1*q3 - q0*q2);
d15321854 6:cc0a54642cdb 63 vy = 2*(q0*q1 + q2*q3);
d15321854 6:cc0a54642cdb 64 vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
d15321854 6:cc0a54642cdb 65
d15321854 6:cc0a54642cdb 66 // error is sum of cross product between reference direction of field and direction measured by sensor
d15321854 6:cc0a54642cdb 67 ex = (ay*vz - az*vy);
d15321854 6:cc0a54642cdb 68 ey = (az*vx - ax*vz);
d15321854 6:cc0a54642cdb 69 ez = (ax*vy - ay*vx);
d15321854 6:cc0a54642cdb 70
d15321854 6:cc0a54642cdb 71 // integral error scaled integral gain
d15321854 6:cc0a54642cdb 72 exInt += ex*Ki;
d15321854 6:cc0a54642cdb 73 eyInt += ey*Ki;
d15321854 6:cc0a54642cdb 74 ezInt += ez*Ki;
d15321854 6:cc0a54642cdb 75
d15321854 6:cc0a54642cdb 76 // adjusted gyroscope measurements
d15321854 6:cc0a54642cdb 77 gx += Kp*ex + exInt;
d15321854 6:cc0a54642cdb 78 gy += Kp*ey + eyInt;
d15321854 6:cc0a54642cdb 79 gz += Kp*ez + ezInt;
d15321854 6:cc0a54642cdb 80
d15321854 6:cc0a54642cdb 81 // integrate quaternion rate and normalise
d15321854 6:cc0a54642cdb 82 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!
d15321854 6:cc0a54642cdb 83 float q1o = q1;
d15321854 6:cc0a54642cdb 84 float q2o = q2;
d15321854 6:cc0a54642cdb 85 float q3o = q3;
d15321854 6:cc0a54642cdb 86 q0 += (-q1o*gx - q2o*gy - q3o*gz)*halfT;
d15321854 6:cc0a54642cdb 87 q1 += (q0o*gx + q2o*gz - q3o*gy)*halfT;
d15321854 6:cc0a54642cdb 88 q2 += (q0o*gy - q1o*gz + q3o*gx)*halfT;
d15321854 6:cc0a54642cdb 89 q3 += (q0o*gz + q1o*gy - q2o*gx)*halfT;
d15321854 6:cc0a54642cdb 90
d15321854 6:cc0a54642cdb 91 // normalise quaternion
d15321854 6:cc0a54642cdb 92 norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
d15321854 6:cc0a54642cdb 93 q0 = q0 / norm;
d15321854 6:cc0a54642cdb 94 q1 = q1 / norm;
d15321854 6:cc0a54642cdb 95 q2 = q2 / norm;
d15321854 6:cc0a54642cdb 96 q3 = q3 / norm;
d15321854 6:cc0a54642cdb 97 }
d15321854 6:cc0a54642cdb 98 void IMU_AHRSupdate(float halfT, float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
d15321854 6:cc0a54642cdb 99 float norm;
d15321854 6:cc0a54642cdb 100 float hx, hy, hz, bx, bz;
d15321854 6:cc0a54642cdb 101 float vx, vy, vz, wx, wy, wz;
d15321854 6:cc0a54642cdb 102 float ex, ey, ez;
d15321854 6:cc0a54642cdb 103
d15321854 6:cc0a54642cdb 104 // auxiliary variables to reduce number of repeated operations
d15321854 6:cc0a54642cdb 105 float q0q0 = q0*q0;
d15321854 6:cc0a54642cdb 106 float q0q1 = q0*q1;
d15321854 6:cc0a54642cdb 107 float q0q2 = q0*q2;
d15321854 6:cc0a54642cdb 108 float q0q3 = q0*q3;
d15321854 6:cc0a54642cdb 109 float q1q1 = q1*q1;
d15321854 6:cc0a54642cdb 110 float q1q2 = q1*q2;
d15321854 6:cc0a54642cdb 111 float q1q3 = q1*q3;
d15321854 6:cc0a54642cdb 112 float q2q2 = q2*q2;
d15321854 6:cc0a54642cdb 113 float q2q3 = q2*q3;
d15321854 6:cc0a54642cdb 114 float q3q3 = q3*q3;
d15321854 6:cc0a54642cdb 115
d15321854 6:cc0a54642cdb 116 // normalise the measurements
d15321854 6:cc0a54642cdb 117 norm = sqrt(ax*ax + ay*ay + az*az);
d15321854 6:cc0a54642cdb 118 if(norm == 0.0f) return;
d15321854 6:cc0a54642cdb 119 ax = ax / norm;
d15321854 6:cc0a54642cdb 120 ay = ay / norm;
d15321854 6:cc0a54642cdb 121 az = az / norm;
d15321854 6:cc0a54642cdb 122 norm = sqrt(mx*mx + my*my + mz*mz);
d15321854 6:cc0a54642cdb 123
d15321854 6:cc0a54642cdb 124 if(norm == 0.0f) return;
d15321854 6:cc0a54642cdb 125 mx = mx / norm;
d15321854 6:cc0a54642cdb 126 my = my / norm;
d15321854 6:cc0a54642cdb 127 mz = mz / norm;
d15321854 6:cc0a54642cdb 128 /*OX=mx;
d15321854 6:cc0a54642cdb 129 OY=my;
d15321854 6:cc0a54642cdb 130 OZ=mz;*/
d15321854 6:cc0a54642cdb 131 // compute reference direction of flux
d15321854 6:cc0a54642cdb 132 hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
d15321854 6:cc0a54642cdb 133 hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
d15321854 6:cc0a54642cdb 134 hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);
d15321854 6:cc0a54642cdb 135 bx = sqrt((hx*hx) + (hy*hy));
d15321854 6:cc0a54642cdb 136 bz = hz;
d15321854 6:cc0a54642cdb 137 // estimated direction of gravity and flux (v and w)
d15321854 6:cc0a54642cdb 138 vx = 2*(q1q3 - q0q2);
d15321854 6:cc0a54642cdb 139 vy = 2*(q0q1 + q2q3);
d15321854 6:cc0a54642cdb 140 vz = q0q0 - q1q1 - q2q2 + q3q3;
d15321854 6:cc0a54642cdb 141 wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
d15321854 6:cc0a54642cdb 142 wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
d15321854 6:cc0a54642cdb 143 wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);
d15321854 6:cc0a54642cdb 144
d15321854 6:cc0a54642cdb 145 // error is sum of cross product between reference direction of fields and direction measured by sensors
d15321854 6:cc0a54642cdb 146 ex = (ay*vz - az*vy) + (my*wz - mz*wy);
d15321854 6:cc0a54642cdb 147 ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
d15321854 6:cc0a54642cdb 148 ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
d15321854 6:cc0a54642cdb 149
d15321854 6:cc0a54642cdb 150
d15321854 6:cc0a54642cdb 151 // integral error scaled integral gain
d15321854 6:cc0a54642cdb 152 exInt = exInt + ex*Ki;
d15321854 6:cc0a54642cdb 153 eyInt = eyInt + ey*Ki;
d15321854 6:cc0a54642cdb 154 ezInt = ezInt + ez*Ki;
d15321854 6:cc0a54642cdb 155
d15321854 6:cc0a54642cdb 156 // adjusted gyroscope measurements
d15321854 6:cc0a54642cdb 157 gx = gx + Kp*ex + exInt;
d15321854 6:cc0a54642cdb 158 gy = gy + Kp*ey + eyInt;
d15321854 6:cc0a54642cdb 159 gz = gz + Kp*ez + ezInt;
d15321854 6:cc0a54642cdb 160
d15321854 6:cc0a54642cdb 161 // integrate quaternion rate and normalise
d15321854 6:cc0a54642cdb 162 q0_A = q0_A + (-q1*gx - q2*gy - q3*gz)*halfT;
d15321854 6:cc0a54642cdb 163 q1_A = q1_A + (q0*gx + q2*gz - q3*gy)*halfT;
d15321854 6:cc0a54642cdb 164 q2_A = q2_A + (q0*gy - q1*gz + q3*gx)*halfT;
d15321854 6:cc0a54642cdb 165 q3_A = q3_A + (q0*gz + q1*gy - q2*gx)*halfT;
d15321854 6:cc0a54642cdb 166
d15321854 6:cc0a54642cdb 167 // normalise quaternion
d15321854 6:cc0a54642cdb 168 norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
d15321854 6:cc0a54642cdb 169 q0_A = q0_A / norm;
d15321854 6:cc0a54642cdb 170 q1_A = q1_A / norm;
d15321854 6:cc0a54642cdb 171 q2_A = q2_A / norm;
d15321854 6:cc0a54642cdb 172 q3_A = q3_A / norm;
d15321854 6:cc0a54642cdb 173 }
d15321854 6:cc0a54642cdb 174
d15321854 6:cc0a54642cdb 175
d15321854 6:cc0a54642cdb 176
d15321854 6:cc0a54642cdb 177
d15321854 6:cc0a54642cdb 178 void Filter_compute(float dt, const float * Gyro_data, const float * Acc_data, const float * Comp_data)
d15321854 6:cc0a54642cdb 179 {
d15321854 6:cc0a54642cdb 180 // IMU/AHRS
d15321854 6:cc0a54642cdb 181
d15321854 6:cc0a54642cdb 182 float d_Gyro_angle[3];
d15321854 6:cc0a54642cdb 183 void get_Acc_angle(const float * Acc_data);
d15321854 6:cc0a54642cdb 184 // IMU/AHRS (from http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/)
d15321854 6:cc0a54642cdb 185 float radGyro[3],Gyro_cal_data; // Gyro in radians per second
d15321854 6:cc0a54642cdb 186
d15321854 6:cc0a54642cdb 187 for(int i=0; i<3; i++)
d15321854 6:cc0a54642cdb 188 radGyro[i] = Gyro_data[i] * 3.14159/ 180;
d15321854 6:cc0a54642cdb 189
d15321854 6:cc0a54642cdb 190 Filter_IMUupdate(dt/2, radGyro[0], radGyro[1], radGyro[2], Acc_data[0], Acc_data[1], Acc_data[2]);
d15321854 6:cc0a54642cdb 191 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]);
d15321854 6:cc0a54642cdb 192
d15321854 6:cc0a54642cdb 193 float rangle[3]; // calculate angles in radians from quternion output, formula from Wiki (http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles)
d15321854 6:cc0a54642cdb 194
d15321854 6:cc0a54642cdb 195 rangle[0] = atan2(2*q0*q1 + 2*q2*q3, 1 - 2*(q1*q1 + q2*q2));
d15321854 6:cc0a54642cdb 196 rangle[1] = asin (2*q0*q2 - 2*q3*q1);
d15321854 6:cc0a54642cdb 197 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
d15321854 6:cc0a54642cdb 198
d15321854 6:cc0a54642cdb 199
d15321854 6:cc0a54642cdb 200 for(int i=0; i<2; i++){ // angle in degree
d15321854 6:cc0a54642cdb 201 angle[i] = rangle[i] * 180 / 3.14159;
d15321854 6:cc0a54642cdb 202 }
d15321854 6:cc0a54642cdb 203 Roll=angle[0];
d15321854 6:cc0a54642cdb 204 Pitch=angle[1];
d15321854 6:cc0a54642cdb 205 //**************************************************Gyro_data[2] filter start
d15321854 6:cc0a54642cdb 206 float GYRO_z=0;
d15321854 6:cc0a54642cdb 207
d15321854 6:cc0a54642cdb 208 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;
d15321854 6:cc0a54642cdb 209 if( count4==1 ){
d15321854 6:cc0a54642cdb 210 GYRO_z_pre_L=GYRO_z_pre;
d15321854 6:cc0a54642cdb 211
d15321854 6:cc0a54642cdb 212 count4=0;
d15321854 6:cc0a54642cdb 213 }
d15321854 6:cc0a54642cdb 214 if( count5==2 ){
d15321854 6:cc0a54642cdb 215 GYRO_z_pre_LL=GYRO_z_pre_L;
d15321854 6:cc0a54642cdb 216
d15321854 6:cc0a54642cdb 217 count5=0;
d15321854 6:cc0a54642cdb 218 }
d15321854 6:cc0a54642cdb 219 if( count6==3 ){
d15321854 6:cc0a54642cdb 220 GYRO_z_pre_LLL=GYRO_z_pre_LL;
d15321854 6:cc0a54642cdb 221
d15321854 6:cc0a54642cdb 222 count6=0;
d15321854 6:cc0a54642cdb 223 }
d15321854 6:cc0a54642cdb 224
d15321854 6:cc0a54642cdb 225
d15321854 6:cc0a54642cdb 226
d15321854 6:cc0a54642cdb 227 count4++;
d15321854 6:cc0a54642cdb 228 count5++;
d15321854 6:cc0a54642cdb 229 count6++;
d15321854 6:cc0a54642cdb 230 GYRO_z_pre=Gyro_data[2];
d15321854 6:cc0a54642cdb 231 Global_GYRO_z=GYRO_z;
d15321854 6:cc0a54642cdb 232 /*pc.printf(" GYRO_z:%10.3f ,count8:%10d ",
d15321854 6:cc0a54642cdb 233 GYRO_z,
d15321854 6:cc0a54642cdb 234 count8
d15321854 6:cc0a54642cdb 235 );*/
d15321854 6:cc0a54642cdb 236 if((count8>5)&&(count8<=805)){
d15321854 6:cc0a54642cdb 237 GYRO_z_total+=GYRO_z;
d15321854 6:cc0a54642cdb 238 }
d15321854 6:cc0a54642cdb 239 if( count8==805 ){
d15321854 6:cc0a54642cdb 240 GYRO_z_offset=GYRO_z_total/800;
d15321854 6:cc0a54642cdb 241 /*pc.printf(" GYRO_z_offset:%10.5f ",
d15321854 6:cc0a54642cdb 242 GYRO_z_offset
d15321854 6:cc0a54642cdb 243 );*/
d15321854 6:cc0a54642cdb 244 GYRO_z_total=0;
d15321854 6:cc0a54642cdb 245 count8=0;
d15321854 6:cc0a54642cdb 246 }
d15321854 6:cc0a54642cdb 247
d15321854 6:cc0a54642cdb 248 count8++;
d15321854 6:cc0a54642cdb 249 //**************************************************Gyro_data[2]'s average filter : answer=GYRO_Z is roughly = 1.05447
d15321854 6:cc0a54642cdb 250
d15321854 6:cc0a54642cdb 251 //************************************************** calculate Yaw
d15321854 6:cc0a54642cdb 252 if( (count11==35) ){
d15321854 6:cc0a54642cdb 253 if( abs(Yaw_pre-Yaw)<1 ){
d15321854 6:cc0a54642cdb 254 Yaw_pre=Yaw_pre;
d15321854 6:cc0a54642cdb 255 }else{
d15321854 6:cc0a54642cdb 256 Yaw_pre=Yaw;
d15321854 6:cc0a54642cdb 257 }
d15321854 6:cc0a54642cdb 258 count11=0;
d15321854 6:cc0a54642cdb 259 }
d15321854 6:cc0a54642cdb 260 count11++;
d15321854 6:cc0a54642cdb 261
d15321854 6:cc0a54642cdb 262 if( count12>=20 ){
d15321854 6:cc0a54642cdb 263 Yaw += (Gyro_data[2]-1.05447) *dt;
d15321854 6:cc0a54642cdb 264 }
d15321854 6:cc0a54642cdb 265 count12++;
d15321854 6:cc0a54642cdb 266 pc.printf(" Yaw:%10.5f ",
d15321854 6:cc0a54642cdb 267 Yaw
d15321854 6:cc0a54642cdb 268 );
d15321854 6:cc0a54642cdb 269 }
d15321854 6:cc0a54642cdb 270 void Mag_Complentary_Filter(float dt, const float * Comp_data)
d15321854 6:cc0a54642cdb 271 {
d15321854 6:cc0a54642cdb 272 float Mag_x=0,Mag_y=0,Mag_z=0;
d15321854 6:cc0a54642cdb 273 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;
d15321854 6:cc0a54642cdb 274 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;
d15321854 6:cc0a54642cdb 275 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;
d15321854 6:cc0a54642cdb 276
d15321854 6:cc0a54642cdb 277
d15321854 6:cc0a54642cdb 278 if( count1==1 ){
d15321854 6:cc0a54642cdb 279 Mag_x_pre_L=Mag_x_pre;
d15321854 6:cc0a54642cdb 280 Mag_y_pre_L=Mag_y_pre;
d15321854 6:cc0a54642cdb 281 Mag_z_pre_L=Mag_z_pre;
d15321854 6:cc0a54642cdb 282 Cal_Mag_x_pre=Cal_Mag_x;
d15321854 6:cc0a54642cdb 283
d15321854 6:cc0a54642cdb 284 count1=0;
d15321854 6:cc0a54642cdb 285 }
d15321854 6:cc0a54642cdb 286 if( count2==2 ){
d15321854 6:cc0a54642cdb 287 Mag_x_pre_LL=Mag_x_pre_L;
d15321854 6:cc0a54642cdb 288 Mag_y_pre_LL=Mag_y_pre_L;
d15321854 6:cc0a54642cdb 289 Mag_z_pre_LL=Mag_z_pre_L;
d15321854 6:cc0a54642cdb 290 Cal_Mag_x_pre_L=Cal_Mag_x_pre;
d15321854 6:cc0a54642cdb 291
d15321854 6:cc0a54642cdb 292 count2=0;
d15321854 6:cc0a54642cdb 293 }
d15321854 6:cc0a54642cdb 294 if( count7==3 ){
d15321854 6:cc0a54642cdb 295 Mag_x_pre_LLL=Mag_x_pre_LL;
d15321854 6:cc0a54642cdb 296 Mag_y_pre_LLL=Mag_y_pre_LL;
d15321854 6:cc0a54642cdb 297 Mag_z_pre_LLL=Mag_z_pre_LL;
d15321854 6:cc0a54642cdb 298 Cal_Mag_x_pre_LL=Cal_Mag_x_pre_L;
d15321854 6:cc0a54642cdb 299
d15321854 6:cc0a54642cdb 300 count7=0;
d15321854 6:cc0a54642cdb 301 }
d15321854 6:cc0a54642cdb 302
d15321854 6:cc0a54642cdb 303
d15321854 6:cc0a54642cdb 304 count1++;
d15321854 6:cc0a54642cdb 305 count2++;
d15321854 6:cc0a54642cdb 306 count7++;
d15321854 6:cc0a54642cdb 307 Mag_x_pre=Comp_data[0];
d15321854 6:cc0a54642cdb 308 Mag_y_pre=Comp_data[1];
d15321854 6:cc0a54642cdb 309 Mag_z_pre=Comp_data[2];
d15321854 6:cc0a54642cdb 310 if( count14>4 ){
d15321854 6:cc0a54642cdb 311 Cal_Mag_x=Mag_x;
d15321854 6:cc0a54642cdb 312 }
d15321854 6:cc0a54642cdb 313 count14++;
d15321854 6:cc0a54642cdb 314
d15321854 6:cc0a54642cdb 315
d15321854 6:cc0a54642cdb 316 //*************************************Mag_ave calculate
d15321854 6:cc0a54642cdb 317 if(count3<=20){
d15321854 6:cc0a54642cdb 318 Mag_x_total+=Mag_x;
d15321854 6:cc0a54642cdb 319 Mag_y_total+=Mag_y;
d15321854 6:cc0a54642cdb 320 }
d15321854 6:cc0a54642cdb 321 if( count3==20){
d15321854 6:cc0a54642cdb 322 Mag_x_ave=Mag_x_total/21;
d15321854 6:cc0a54642cdb 323 Mag_y_ave=Mag_y_total/21;
d15321854 6:cc0a54642cdb 324 /*pc.printf(" Mag_x_ave:%10.5f ,Mag_y_ave:%10.5f ",
d15321854 6:cc0a54642cdb 325 Mag_x_ave,
d15321854 6:cc0a54642cdb 326 Mag_y_ave
d15321854 6:cc0a54642cdb 327 );*/
d15321854 6:cc0a54642cdb 328 Mag_x_total=0;
d15321854 6:cc0a54642cdb 329 Mag_y_total=0;
d15321854 6:cc0a54642cdb 330 count3=0;
d15321854 6:cc0a54642cdb 331
d15321854 6:cc0a54642cdb 332
d15321854 6:cc0a54642cdb 333 }
d15321854 6:cc0a54642cdb 334 count3++;
d15321854 6:cc0a54642cdb 335
d15321854 6:cc0a54642cdb 336 //********************************ROT_check start
d15321854 6:cc0a54642cdb 337
d15321854 6:cc0a54642cdb 338 float v_length,v_length_ave,MagVector_angle;
d15321854 6:cc0a54642cdb 339 v_length=sqrt( Mag_x*Mag_x + Mag_y*Mag_y );
d15321854 6:cc0a54642cdb 340 v_length_ave=sqrt( Mag_x_ave*Mag_x_ave + Mag_y_ave*Mag_y_ave );
d15321854 6:cc0a54642cdb 341
d15321854 6:cc0a54642cdb 342 MagVector_angle=acos(( Mag_x*Mag_x_ave + Mag_y*Mag_y_ave )/(v_length*v_length_ave))*57.3;
d15321854 6:cc0a54642cdb 343
d15321854 6:cc0a54642cdb 344 if( count9==3 ){
d15321854 6:cc0a54642cdb 345 Global_mag_vector_angle=MagVector_angle;
d15321854 6:cc0a54642cdb 346 count9=0;
d15321854 6:cc0a54642cdb 347 }
d15321854 6:cc0a54642cdb 348 count9++;
d15321854 6:cc0a54642cdb 349
d15321854 6:cc0a54642cdb 350
d15321854 6:cc0a54642cdb 351 if( (abs(Global_mag_vector_angle-MagVector_angle)<5) && (abs(Global_GYRO_z)<5) ){
d15321854 6:cc0a54642cdb 352 Count_mag_check++;
d15321854 6:cc0a54642cdb 353
d15321854 6:cc0a54642cdb 354 }else{ Count_mag_check=0; }
d15321854 6:cc0a54642cdb 355
d15321854 6:cc0a54642cdb 356 if( Count_mag_check==30 ){
d15321854 6:cc0a54642cdb 357 Yaw=Yaw_pre;
d15321854 6:cc0a54642cdb 358 Count_mag_check=0;
d15321854 6:cc0a54642cdb 359 }
d15321854 6:cc0a54642cdb 360 float ABS_CHECK=abs(Global_mag_vector_angle-MagVector_angle);
d15321854 6:cc0a54642cdb 361 //********************************Theta_check end
d15321854 6:cc0a54642cdb 362 /*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 ",
d15321854 6:cc0a54642cdb 363 ABS_CHECK,
d15321854 6:cc0a54642cdb 364 Cal_Mag_x_pre_LL,
d15321854 6:cc0a54642cdb 365 Mag_x,
d15321854 6:cc0a54642cdb 366 Count_mag_check,
d15321854 6:cc0a54642cdb 367 Yaw_pre,
d15321854 6:cc0a54642cdb 368 Yaw
d15321854 6:cc0a54642cdb 369 );*/
d15321854 6:cc0a54642cdb 370
d15321854 6:cc0a54642cdb 371 }
kylongmu 0:58f9d4556df7 372 int main(){
kylongmu 0:58f9d4556df7 373 pc.baud(115200);
kylongmu 5:5839d1b118bc 374 if(imu.init(1,BITS_DLPF_CFG_188HZ)){ //INIT the mpu9250
d15321854 6:cc0a54642cdb 375 pc.printf("\nCouldn't initialize MPU9250 via SPI!");
kylongmu 0:58f9d4556df7 376 }
d15321854 6:cc0a54642cdb 377 pc.printf("\nWHOAMI=0x%2x\n",imu.whoami()); //output the I2C address to know if SPI is working, it should be 104
kylongmu 1:7497c7952470 378 wait(1);
d15321854 6:cc0a54642cdb 379 pc.printf("Gyro_scale=%u\n",imu.set_gyro_scale(BITS_FS_2000DPS)); //Set full scale range for gyros
kylongmu 0:58f9d4556df7 380 wait(1);
d15321854 6:cc0a54642cdb 381 pc.printf("Acc_scale=%u\n",imu.set_acc_scale(BITS_FS_16G)); //Set full scale range for accs
kylongmu 1:7497c7952470 382 wait(1);
d15321854 6:cc0a54642cdb 383 pc.printf("AK8963 WHIAM=0x%2x\n",imu.AK8963_whoami());
kylongmu 1:7497c7952470 384 wait(0.1);
kylongmu 5:5839d1b118bc 385 imu.AK8963_calib_Magnetometer();
kylongmu 0:58f9d4556df7 386 while(1) {
d15321854 6:cc0a54642cdb 387
d15321854 6:cc0a54642cdb 388 LoopTimer.start();
kylongmu 0:58f9d4556df7 389 wait(0.1);
d15321854 6:cc0a54642cdb 390
d15321854 6:cc0a54642cdb 391 SensorTimer.start();
d15321854 6:cc0a54642cdb 392 Times[1] = LoopTimer.read(); // 197us
d15321854 6:cc0a54642cdb 393 SensorTimer.stop(); // stop time for measuring sensors
d15321854 6:cc0a54642cdb 394 dt_sensors = SensorTimer.read();
d15321854 6:cc0a54642cdb 395 SensorTimer.reset();
d15321854 6:cc0a54642cdb 396 // meassure dt since last measurement for the filter
d15321854 6:cc0a54642cdb 397 dt = LoopTimer.read(); // time in s since last loop
d15321854 6:cc0a54642cdb 398 LoopTimer.reset();
d15321854 6:cc0a54642cdb 399
d15321854 6:cc0a54642cdb 400
kylongmu 4:6bfddd447c27 401 imu.read_all();
d15321854 6:cc0a54642cdb 402
d15321854 6:cc0a54642cdb 403 Mag_Complentary_Filter(dt,imu.Magnetometer);
d15321854 6:cc0a54642cdb 404 Filter_compute(dt, imu.gyroscope_data, imu.accelerometer_data, imu.Magnetometer);
d15321854 6:cc0a54642cdb 405
d15321854 6:cc0a54642cdb 406 pc.printf("Roll:%10.3f,Pitch:%10.3f,Yaw:%10.3f \n",
d15321854 6:cc0a54642cdb 407 Roll,
d15321854 6:cc0a54642cdb 408 Pitch,
d15321854 6:cc0a54642cdb 409 Yaw
kylongmu 1:7497c7952470 410 );
d15321854 6:cc0a54642cdb 411
kylongmu 0:58f9d4556df7 412 }
kylongmu 0:58f9d4556df7 413 }