btl143/QD4C-firmware
4
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

更新陀螺仪相关设置

Browse Source
This commit is contained in:
CaoWangrenbo
2023-12-24 15:39:44 +08:00
parent c5e64f3cb0
commit 815a9821a0
4 changed files with 58 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

69
app/by_imu.c
View File

@@ -2,7 +2,7 @@
#include "zf_common_headfile.h"
#include <math.h>
#define delta_T 0.001f // 1ms计算一次
#define delta_T 0.002f // 2ms 计算一次
float I_ex, I_ey, I_ez; // 误差积分
@@ -15,7 +15,7 @@ gyro_param_t GyroOffset;
bool GyroOffset_init = 0;
float param_Kp = 0.17; // 加速度计的收敛速率比例增益 0.17
float param_Ki = 0.008; // 陀螺仪收敛速率的积分增益 0.004
float param_Ki = 0.004; // 陀螺仪收敛速率的积分增益 0.004
float fast_sqrt(float x)
{
@@ -30,35 +30,56 @@ float fast_sqrt(float x)
void gyroOffset_init(void) /////////陀螺仪零飘初始化
{
float xdata_temp = 0;
float ydata_temp = 0;
float zdata_temp = 0;
imu660ra_get_gyro();
xdata_temp = imu660ra_gyro_x;
ydata_temp = imu660ra_gyro_y;
zdata_temp = imu660ra_gyro_z;
GyroOffset.Xdata = 0;
GyroOffset.Ydata = 0;
GyroOffset.Zdata = 0;
for (uint16_t i = 0; i < 500; ++i) {
imu660ra_get_acc();
for (uint16_t i = 0; i < 5000; i++) {
imu660ra_get_gyro();
GyroOffset.Xdata += imu660ra_gyro_x;
GyroOffset.Ydata += imu660ra_gyro_y;
GyroOffset.Zdata += imu660ra_gyro_z;
system_delay_ms(1);
xdata_temp = imu660ra_gyro_x * 0.3f + xdata_temp * 0.7f;
ydata_temp = imu660ra_gyro_y * 0.3f + ydata_temp * 0.7f;
zdata_temp = imu660ra_gyro_z * 0.3f + zdata_temp * 0.7f;
GyroOffset.Xdata += xdata_temp;
GyroOffset.Ydata += ydata_temp;
GyroOffset.Zdata += zdata_temp;
system_delay_ms(5);
}
GyroOffset.Xdata /= 500.0f;
GyroOffset.Ydata /= 500.0f;
GyroOffset.Zdata /= 500.0f;
GyroOffset.Xdata /= 5000.0f;
GyroOffset.Ydata /= 5000.0f;
GyroOffset.Zdata /= 5000.0f;
// GyroOffset.Xdata = -1.034f;
// GyroOffset.Ydata = -2.21f;
// GyroOffset.Zdata = -3.15f;
GyroOffset_init = 1;
}
// 转化为实际物理值
void ICM_getValues()
{
#define alpha 0.3f
icm_data.acc_x = imu660ra_acc_transition(imu660ra_acc_x) + icm_data.acc_x * (1 - alpha);
icm_data.acc_y = imu660ra_acc_transition(imu660ra_acc_y) + icm_data.acc_y * (1 - alpha);
icm_data.acc_z = imu660ra_acc_transition(imu660ra_acc_z) + icm_data.acc_z * (1 - alpha);
#define alpha 0.2738f
// icm_data.acc_x = imu660ra_acc_x * alpha + icm_data.acc_x * (1 - alpha);
// icm_data.acc_y = imu660ra_acc_y * alpha + icm_data.acc_y * (1 - alpha);
// icm_data.acc_z = imu660ra_acc_z * alpha + icm_data.acc_z * (1 - alpha);
icm_data.gyro_x = (imu660ra_gyro_transition(imu660ra_gyro_x)-GyroOffset.Xdata) / 57.3f;
icm_data.gyro_y = (imu660ra_gyro_transition(imu660ra_gyro_y)-GyroOffset.Ydata) / 57.3f;
icm_data.gyro_z = (imu660ra_gyro_transition(imu660ra_gyro_z)-GyroOffset.Zdata) / 57.3f;
icm_data.acc_x = imu660ra_acc_x;
icm_data.acc_y = imu660ra_acc_y;
icm_data.acc_z = imu660ra_acc_z;
icm_data.gyro_x = icm_data.gyro_x * alpha + ((imu660ra_gyro_x - GyroOffset.Xdata) / 939.65f) * (1 - alpha);
icm_data.gyro_y = icm_data.gyro_y * alpha + ((imu660ra_gyro_y - GyroOffset.Ydata) / 939.65f) * (1 - alpha);
icm_data.gyro_z = icm_data.gyro_z * alpha + ((imu660ra_gyro_z - GyroOffset.Zdata) / 939.65f) * (1 - alpha);
}
// 互补滤波
@@ -74,9 +95,9 @@ void ICM_AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az)
float q0q0 = q0 * q0;
float q0q1 = q0 * q1;
float q0q2 = q0 * q2;
float q0q3 = q0 * q3;
// float q0q3 = q0 * q3;
float q1q1 = q1 * q1;
float q1q2 = q1 * q2;
// float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q2q2 = q2 * q2;
float q2q3 = q2 * q3;
@@ -92,8 +113,8 @@ void ICM_AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az)
// 根据当前四元数的姿态值来估算出各重力分量。用于和加速计实际测量出来的各重力分量进行对比，从而实现对四轴姿态的修正
vx = 2.0f * (q1q3 - q0q2);
vy = 2.0f * (q0q1 + q2q3);
// vz = q0q0 - q1q1 - q2q2 + q3q3;
vz=1.0f-2.0f*q1q1-2.0f*q2q2;
vz = q0q0 - q1q1 - q2q2 + q3q3;
// vz = 1.0f - 2.0f * q1q1 - 2.0f * q2q2;
// vz = (q0*q0-0.5f+q3 * q3) * 2;
// 叉积来计算估算的重力和实际测量的重力这两个重力向量之间的误差。
@@ -101,7 +122,7 @@ void ICM_AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az)
ey = az * vx - ax * vz;
ez = ax * vy - ay * vx;
// 用叉乘误差来做PI修正陀螺零偏，
// 用叉乘误差来做 PI 修正陀螺零偏，
// 通过调节 param_Kp，param_Ki 两个参数，
// 可以控制加速度计修正陀螺仪积分姿态的速度。
I_ex += halfT * ex; // integral error scaled by Ki
@@ -114,7 +135,7 @@ void ICM_AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az)
/*数据修正完成，下面是四元数微分方程*/
// 四元数微分方程，其中halfT为测量周期的1/2，gx gy gz为陀螺仪角速度，以下都是已知量，这里使用了一阶龙哥库塔求解四元数微分方程
// 四元数微分方程，其中 halfT 为测量周期的 1/2，gx gy 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;