/************************************************************************************************ * 程序版本：V3.0 * 程序日期：2022-11-3 * 程序作者： ************************************************************************************************/ #include "Imu.h" #include "main.h" #define Kp_New 0.9f //互补滤波当前数据的权重 #define Kp_Old 0.1f //互补滤波历史数据的权重 #define Acc_Gain 0.0001220f //加速度变成G (初始化加速度满量程-+4g LSBa = 2*4/65535.0) #define Gyro_Gain 0.0609756f //角速度变成度 (初始化陀螺仪满量程+-2000 LSBg = 2*2000/65535.0) #define Gyro_Gr 0.0010641f //角速度变成弧度(3.1415/180 * LSBg) //kp=ki=0 就是完全相信陀螺仪 #define Kp 1.5f // proportional gain governs rate of convergence to accelerometer/magnetometer //比例增益控制加速度计，磁力计的收敛速率 #define Ki 0.005f // integral gain governs rate of convergence of gyroscope biases //积分增益控制陀螺偏差的收敛速度 #define halfT 0.005f // half the sample period 采样周期的一半 static volatile float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // quaternion elements representing the estimated orientation static volatile float exInt = 0, eyInt = 0, ezInt = 0; // scaled integral error /* static float MedianValue(float *value_buf, unsigned int size) { unsigned int i,j; float temp; //排序采用冒泡法 for (j=0;j<=size;j++){ for (i=0;i<=size-j;i++){ if (value_buf[i] > value_buf[i+1]){ temp = value_buf[i]; value_buf[i] = value_buf[i+1]; value_buf[i+1] = temp; } } } return value_buf[(size-1)/2]; } */ static float MedianValue(float *value_buf, unsigned int size) { unsigned int i; float temp = 0; for (i=0; i=Imu_AccFilterSize)cnt=0; return MedianValue(value_buf, Imu_AccFilterSize); } static float Acc_Filter_Y(float in_data) { static float value_buf[Imu_AccFilterSize] = {0}; static unsigned char cnt = 0; value_buf[cnt] = in_data; cnt++; if (cnt>=Imu_AccFilterSize)cnt=0; return MedianValue(value_buf, Imu_AccFilterSize); } static float Acc_Filter_Z(float in_data) { static float value_buf[Imu_AccFilterSize] = {0}; static unsigned char cnt = 0; value_buf[cnt] = in_data; cnt++; if (cnt>=Imu_AccFilterSize)cnt=0; return MedianValue(value_buf, Imu_AccFilterSize); } static float Mag_Filter_X(float in_data) { static float value_buf[Imu_MagFilterSize] = {0}; static unsigned char cnt = 0; value_buf[cnt] = in_data; cnt++; if (cnt>=Imu_MagFilterSize)cnt=0; return MedianValue(value_buf, Imu_MagFilterSize); } static float Mag_Filter_Y(float in_data) { static float value_buf[Imu_MagFilterSize] = {0}; static unsigned char cnt = 0; value_buf[cnt] = in_data; cnt++; if (cnt>=Imu_MagFilterSize)cnt=0; return MedianValue(value_buf, Imu_MagFilterSize); } static float Mag_Filter_Z(float in_data) { static float value_buf[Imu_MagFilterSize] = {0}; static unsigned char cnt = 0; value_buf[cnt] = in_data; cnt++; if (cnt>=Imu_MagFilterSize)cnt=0; return MedianValue(value_buf, Imu_MagFilterSize); } static float Gyro_Filter_X(float in_data) { static float value_buf[Imu_GyroFilterSize] = {0}; static unsigned char cnt = 0; value_buf[cnt] = in_data; cnt++; if (cnt>=Imu_GyroFilterSize)cnt=0; return MedianValue(value_buf, Imu_GyroFilterSize); } static float Gyro_Filter_Y(float in_data) { static float value_buf[Imu_GyroFilterSize] = {0}; static unsigned char cnt = 0; value_buf[cnt] = in_data; cnt++; if (cnt>=Imu_GyroFilterSize)cnt=0; return MedianValue(value_buf, Imu_GyroFilterSize); } static float Gyro_Filter_Z(float in_data) { static float value_buf[Imu_GyroFilterSize] = {0}; static unsigned char cnt = 0; value_buf[cnt] = in_data; cnt++; if (cnt>=Imu_GyroFilterSize)cnt=0; return MedianValue(value_buf, Imu_GyroFilterSize); } static float invSqrt(float x) { float halfx = 0.5f * x; float y = x; long i = *(long*)&y; i = 0x5f3759df - (i>>1); y = *(float*)&i; y = y * (1.5f - (halfx * y * y)); return y; } static void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) { float vx, vy, vz; float ex, ey, ez; float norm; 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; if(ax*ay*az==0) return; //加速度计<测量>的重力加速度向量(机体坐标系) norm = invSqrt(ax*ax + ay*ay + az*az); ax = ax * norm; ay = ay * norm; az = az * norm; // printf("ax=%0.2f ay=%0.2f az=%0.2f\r\n",ax,ay,az); //陀螺仪积分<估计>重力向量(机体坐标系) vx = 2*(q1q3 - q0q2); //矩阵(3,1)项 vy = 2*(q0q1 + q2q3); //矩阵(3,2)项 vz = q0q0 - q1q1 - q2q2 + q3q3 ; //矩阵(3,3)项 //向量叉乘所得的值 ex = (ay*vz - az*vy); ey = (az*vx - ax*vz); ez = (ax*vy - ay*vx); //用上面求出误差进行积分 exInt = exInt + ex * Ki; eyInt = eyInt + ey * Ki; ezInt = ezInt + ez * Ki; //将误差PI后补偿到陀螺仪 gx = gx + Kp*ex + exInt; gy = gy + Kp*ey + eyInt; gz = gz + Kp*ez + ezInt;//这里的gz由于没有观测者进行矫正会产生漂移，表现出来的就是积分自增或自减 //四元素的微分方程 q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT; q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT; q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT; q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT; //单位化四元数 norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); q0 = q0 * norm; q1 = q1 * norm; q2 = q2 * norm; q3 = q3 * norm; // 矩阵表达式 // matrix[0] = q0q0 + q1q1 - q2q2 - q3q3; // 11 // matrix[1] = 2.f * (q1q2 + q0q3); // 12 // matrix[2] = 2.f * (q1q3 - q0q2); // 13 // matrix[3] = 2.f * (q1q2 - q0q3); // 21 // matrix[4] = q0q0 - q1q1 + q2q2 - q3q3; // 22 // matrix[5] = 2.f * (q2q3 + q0q1); // 23 // matrix[6] = 2.f * (q1q3 + q0q2); // 31 // matrix[7] = 2.f * (q2q3 - q0q1); // 32 // matrix[8] = q0q0 - q1q1 - q2q2 + q3q3; // 33 //下一步是四元数转换成欧拉角(Z->Y->X) } #define ALPHA 0.01 void IMU_Update(float *gy,float *acc, float *mag, RadDp Rad, DegDp Deg) { static volatile double Phi,Theta, Psi, Gz2, By2, Bz2, Bx3; volatile float Gyr[XYZ], Acc[XYZ], Mag[XYZ]; Gyr[X] = gy[X]; Gyr[Y] = gy[Y]; Gyr[Z] = gy[Z]; Acc[X] = acc[X]; Acc[Y] = acc[Y]; Acc[Z] = acc[Z]; Mag[X] = -mag[X]; Mag[Y] = -mag[Y]; Mag[Z] = -mag[Z]; // IMUupdate(Gyr[X], Gyr[Y], Gyr[Z], Acc[X], Acc[Y], Acc[Z]); // Phi = -atan2(2.f * q0*q1 + 2.f * q2*q3, 1 - 2.f * q1*q1 - 2.f * q2*q2); // Theta = -asin(2.f * (q0*q2 - q1*q3)); // volatile float xh = Mag[0]*cos(Theta) + Mag[1]*sin(Phi)*sin(Theta) + Mag[2]*cos(Phi)*sin(Theta); // volatile float yh = Mag[1]*cos(Phi) - Mag[2]*sin(Phi); // Psi = atan2(yh, xh); Phi = atan2(Acc[Y], (Acc[Z] + Acc[X] * ALPHA)); //Phi = atan2(Acc[Y], Acc[Z]); Gz2 = Acc[Y] * sin(Phi) + Acc[Z] * cos(Phi); Theta = atan(-Acc[X] / Gz2); By2 = Mag[Z] * sin(Phi) - Mag[Y] * cos(Phi); Bz2 = Mag[Y] * sin(Phi) + Mag[Z] * cos(Phi); Bx3 = Mag[X] * cos(Theta) + Bz2 * sin(Theta); Psi = atan2(By2, Bx3); Rad->roll = Phi; Rad->pitch = Theta; Rad->yaw = Psi; Deg->roll = Rad->roll * RAD_TO_DEG; Deg->pitch = Rad->pitch * RAD_TO_DEG; Deg->yaw = Rad->yaw * RAD_TO_DEG; }