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

65
app/by_imu.c
View File

@@ -2,7 +2,7 @@
#include "zf_common_headfile.h" #include "zf_common_headfile.h"
#include <math.h> #include <math.h>
#define delta_T 0.001f // 1ms计算一次 #define delta_T 0.002f // 2ms 计算一次
float I_ex, I_ey, I_ez; // 误差积分 float I_ex, I_ey, I_ez; // 误差积分
@@ -15,7 +15,7 @@ gyro_param_t GyroOffset;
bool GyroOffset_init = 0; bool GyroOffset_init = 0;
float param_Kp = 0.17; // 加速度计的收敛速率比例增益 0.17 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) float fast_sqrt(float x)
{ {
@@ -30,35 +30,56 @@ float fast_sqrt(float x)
void gyroOffset_init(void) /////////陀螺仪零飘初始化 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.Xdata = 0;
GyroOffset.Ydata = 0; GyroOffset.Ydata = 0;
GyroOffset.Zdata = 0; GyroOffset.Zdata = 0;
for (uint16_t i = 0; i < 500; ++i) { for (uint16_t i = 0; i < 5000; i++) {
imu660ra_get_acc();
imu660ra_get_gyro(); imu660ra_get_gyro();
GyroOffset.Xdata += imu660ra_gyro_x;
GyroOffset.Ydata += imu660ra_gyro_y; xdata_temp = imu660ra_gyro_x * 0.3f + xdata_temp * 0.7f;
GyroOffset.Zdata += imu660ra_gyro_z; ydata_temp = imu660ra_gyro_y * 0.3f + ydata_temp * 0.7f;
system_delay_ms(1); 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.Xdata /= 5000.0f;
GyroOffset.Ydata /= 500.0f; GyroOffset.Ydata /= 5000.0f;
GyroOffset.Zdata /= 500.0f; GyroOffset.Zdata /= 5000.0f;
// GyroOffset.Xdata = -1.034f;
// GyroOffset.Ydata = -2.21f;
// GyroOffset.Zdata = -3.15f;
GyroOffset_init = 1; GyroOffset_init = 1;
} }
// 转化为实际物理值 // 转化为实际物理值
void ICM_getValues() void ICM_getValues()
{ {
#define alpha 0.3f #define alpha 0.2738f
icm_data.acc_x = imu660ra_acc_transition(imu660ra_acc_x) + icm_data.acc_x * (1 - alpha); // icm_data.acc_x = imu660ra_acc_x * alpha + 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_y = imu660ra_acc_y * alpha + icm_data.acc_y * (1 - alpha);
icm_data.acc_z = imu660ra_acc_transition(imu660ra_acc_z) + icm_data.acc_z * (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.acc_x = imu660ra_acc_x;
icm_data.gyro_y = (imu660ra_gyro_transition(imu660ra_gyro_y)-GyroOffset.Ydata) / 57.3f; icm_data.acc_y = imu660ra_acc_y;
icm_data.gyro_z = (imu660ra_gyro_transition(imu660ra_gyro_z)-GyroOffset.Zdata) / 57.3f; 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 q0q0 = q0 * q0;
float q0q1 = q0 * q1; float q0q1 = q0 * q1;
float q0q2 = q0 * q2; float q0q2 = q0 * q2;
float q0q3 = q0 * q3; // float q0q3 = q0 * q3;
float q1q1 = q1 * q1; float q1q1 = q1 * q1;
float q1q2 = q1 * q2; // float q1q2 = q1 * q2;
float q1q3 = q1 * q3; float q1q3 = q1 * q3;
float q2q2 = q2 * q2; float q2q2 = q2 * q2;
float q2q3 = q2 * q3; 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); vx = 2.0f * (q1q3 - q0q2);
vy = 2.0f * (q0q1 + q2q3); vy = 2.0f * (q0q1 + q2q3);
// vz = q0q0 - q1q1 - q2q2 + q3q3; vz = q0q0 - q1q1 - q2q2 + q3q3;
vz=1.0f-2.0f*q1q1-2.0f*q2q2; // vz = 1.0f - 2.0f * q1q1 - 2.0f * q2q2;
// vz = (q0*q0-0.5f+q3 * q3) * 2; // vz = (q0*q0-0.5f+q3 * q3) * 2;
// 叉积来计算估算的重力和实际测量的重力这两个重力向量之间的误差。 // 叉积来计算估算的重力和实际测量的重力这两个重力向量之间的误差。

3
app/by_imu.h
View File

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

4
app/main.c
View File

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