299 lines
7.7 KiB
C
299 lines
7.7 KiB
C
|
/************************************************************************************************
|
||
|
|
* 程序版本：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<size; i++)
|
||
|
|
temp += value_buf[i];
|
||
|
|
|
||
|
|
return temp/size;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
static float Acc_Filter_X(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_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;
|
||
|
|
}
|
||
|
|
|