Merge pull request '更新陀螺仪相关设置' (#9) from imu_test into master

Reviewed-on: http://git.isthmus.tk:441/btl143/firmware_violet_zf/pulls/9

app/by_imu.c

@@ -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;
 
     // 叉积来计算估算的重力和实际测量的重力这两个重力向量之间的误差。

app/by_imu.h

@@ -1,7 +1,7 @@
 #ifndef _BY_IMU_H__
 #define _BY_IMU_H__
 
-#include "stdio.h"
+#include <stdio.h>
 #include "ch32v30x.h"
 typedef struct {
     float gyro_x;
@@ -33,6 +33,7 @@ typedef struct {
 
 extern icm_param_t icm_data;
 extern euler_param_t eulerAngle;
+extern gyro_param_t GyroOffset;
 
 void gyroOffset_init(void);
 

app/main.c

@@ -102,8 +102,10 @@ int main(void)
     cw_servo_init();
     while (imu660ra_init())
         ;
+
+    system_delay_ms(2000);
     gyroOffset_init();
-    pit_ms_init(TIM6_PIT, 1);
+    pit_ms_init(TIM6_PIT, 2);
 
     while (1) {
         // while (frame_count < 20) {