YuanHongbin/LaserTracing
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

初始版本

Browse Source
This commit is contained in:
YuanHongbin
2026-04-23 14:02:03 +08:00
parent d68cb4273f
commit bbd563b069
121 changed files with 94711 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Module/Imu/Imu.c
+298
View File
@@ -0,0 +1,298 @@
/************************************************************************************************
* 程序版本：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;
}
Module/Imu/Imu.h
+24
View File
@@ -0,0 +1,24 @@
#ifndef _IMU_H_
#define _IMU_H_
#include <math.h>
#define PI 3.14159265358979323846f
#define RAD_TO_DEG 57.2957795 // 弧度到度的转换系数
#define DEG_TO_RAD 0.0174533 // 度到弧度的转换系数
// 定义欧拉角结构体
typedef struct {
volatile float roll, pitch, yaw;//弧度
}Rad_T, *RadDp;
typedef struct {
volatile float roll, pitch, yaw;//角度
}Deg_T, *DegDp;
#define Imu_AccFilterSize 10
#define Imu_MagFilterSize 20
#define Imu_GyroFilterSize 10
void IMU_Update(float *gy,float *acc, float *mag, RadDp Rad, DegDp Deg);
#endif