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 "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;
// 叉积来计算估算的重力和实际测量的重力这两个重力向量之间的误差。 // 叉积来计算估算的重力和实际测量的重力这两个重力向量之间的误差。
@@ -101,7 +122,7 @@ void ICM_AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az)
ey = az * vx - ax * vz; ey = az * vx - ax * vz;
ez = ax * vy - ay * vx; ez = ax * vy - ay * vx;
// 用叉乘误差来做PI修正陀螺零偏， // 用叉乘误差来做 PI 修正陀螺零偏，
// 通过调节 param_Kp，param_Ki 两个参数， // 通过调节 param_Kp，param_Ki 两个参数，
// 可以控制加速度计修正陀螺仪积分姿态的速度。 // 可以控制加速度计修正陀螺仪积分姿态的速度。
I_ex += halfT * ex; // integral error scaled by 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; q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * halfT;
q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * halfT; q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * halfT;
q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * halfT; q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * halfT;

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);

18
app/main.c
View File

@@ -2,11 +2,11 @@
* CH32V307VCT6 Opensourec Library <20><><EFBFBD><EFBFBD>CH32V307VCT6 <20><>Դ<EFBFBD>⣩<EFBFBD><E2A3A9>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD>ڹٷ<DAB9> SDK <20>ӿڵĵ<DAB5><C4B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Դ<EFBFBD><D4B4> * CH32V307VCT6 Opensourec Library <20><><EFBFBD><EFBFBD>CH32V307VCT6 <20><>Դ<EFBFBD>⣩<EFBFBD><E2A3A9>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD>ڹٷ<DAB9> SDK <20>ӿڵĵ<DAB5><C4B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Դ<EFBFBD><D4B4>
* Copyright (c) 2022 SEEKFREE <20><><EFBFBD>ɿƼ<C9BF> * Copyright (c) 2022 SEEKFREE <20><><EFBFBD>ɿƼ<C9BF>
* *
* <20><><EFBFBD>ļ<EFBFBD><C4BC><EFBFBD>CH32V307VCT6 <20><>Դ<EFBFBD><D4B4><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD> * <20><><EFBFBD>ļ<EFBFBD><C4BC><EFBFBD> CH32V307VCT6 <20><>Դ<EFBFBD><D4B4><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>
* *
* CH32V307VCT6 <20><>Դ<EFBFBD><D4B4> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> * CH32V307VCT6 <20><>Դ<EFBFBD><D4B4> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* <20><><EFBFBD><EFBFBD><EFBFBD>Ը<EFBFBD><D4B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ᷢ<EFBFBD><E1B7A2><EFBFBD><EFBFBD> GPL<50><4C>GNU General Public License<73><65><EFBFBD><EFBFBD> GNUͨ<55>ù<EFBFBD><C3B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>֤<EFBFBD><D6A4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> * <20><><EFBFBD><EFBFBD><EFBFBD>Ը<EFBFBD><D4B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ᷢ<EFBFBD><E1B7A2><EFBFBD><EFBFBD> GPL<50><4C>GNU General Public License<73><65><EFBFBD><EFBFBD> GNU ͨ<EFBFBD>ù<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>֤<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* <20><> GPL <20>ĵ<EFBFBD>3<EFBFBD>棨<EFBFBD><EFBFBD> GPL3.0<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѡ<EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD><EFBFBD>κκ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>İ汾<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>·<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>/<2F><><EFBFBD>޸<EFBFBD><DEB8><EFBFBD> * <20><> GPL <20>ĵ<EFBFBD> 3 <EFBFBD>棨<EFBFBD><EFBFBD> GPL3.0<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѡ<EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD><EFBFBD>κκ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>İ汾<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>·<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>/<2F><><EFBFBD>޸<EFBFBD><DEB8><EFBFBD>
* *
* <20><><EFBFBD><EFBFBD>Դ<EFBFBD><D4B4><EFBFBD>ķ<EFBFBD><C4B7><EFBFBD><EFBFBD><EFBFBD>ϣ<EFBFBD><CFA3><EFBFBD><EFBFBD><EFBFBD>ܷ<EFBFBD><DCB7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD>δ<EFBFBD><CEB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>κεı<CEB5>֤ * <20><><EFBFBD><EFBFBD>Դ<EFBFBD><D4B4><EFBFBD>ķ<EFBFBD><C4B7><EFBFBD><EFBFBD><EFBFBD>ϣ<EFBFBD><CFA3><EFBFBD><EFBFBD><EFBFBD>ܷ<EFBFBD><DCB7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD>δ<EFBFBD><CEB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>κεı<CEB5>֤
* <20><><EFBFBD><EFBFBD>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ի<EFBFBD><D4BB>ʺ<EFBFBD><CABA>ض<EFBFBD><D8B6><EFBFBD>;<EFBFBD>ı<EFBFBD>֤ * <20><><EFBFBD><EFBFBD>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ի<EFBFBD><D4BB>ʺ<EFBFBD><CABA>ض<EFBFBD><D8B6><EFBFBD>;<EFBFBD>ı<EFBFBD>֤
@@ -30,7 +30,7 @@
* *
* <20>޸ļ<DEB8>¼ * <20>޸ļ<DEB8>¼
* <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> <20><>ע * <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> <20><>ע
* 2022-09-15 <20><>W first version * 2022-09-15 <20><> W first version
********************************************************************************************************************/ ********************************************************************************************************************/
#include "zf_common_headfile.h" #include "zf_common_headfile.h"
#include "gl_headfile.h" #include "gl_headfile.h"
@@ -52,7 +52,7 @@ sint32 pts_resample_left_count, pts_resample_right_count;
float32 mid_left[PT_MAXLEN][2], mid_right[PT_MAXLEN][2]; float32 mid_left[PT_MAXLEN][2], mid_right[PT_MAXLEN][2];
sint32 mid_left_count, mid_right_count; sint32 mid_left_count, mid_right_count;
// <20><><EFBFBD>ұ<EFBFBD><D2B1>߾ֲ<DFBE><D6B2>Ƕȱ仯<C8B1><E4BBAF>+<EFBFBD>Ǽ<EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD> // <20><><EFBFBD>ұ<EFBFBD><D2B1>߾ֲ<DFBE><D6B2>Ƕȱ仯<C8B1><E4BBAF> + <EFBFBD>Ǽ<EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float angle_new_left[PT_MAXLEN]; float angle_new_left[PT_MAXLEN];
float angle_new_right[PT_MAXLEN]; float angle_new_right[PT_MAXLEN];
int angle_new_left_num, angle_new_right_num; int angle_new_left_num, angle_new_right_num;
@@ -63,7 +63,7 @@ float angle_left[PT_MAXLEN];
float angle_right[PT_MAXLEN]; float angle_right[PT_MAXLEN];
int angle_left_num, angle_right_num; int angle_left_num, angle_right_num;
// L<>ǵ<EFBFBD> // L <EFBFBD>ǵ<EFBFBD>
int Lpt0_rpts0s_id, Lpt1_rpts1s_id; int Lpt0_rpts0s_id, Lpt1_rpts1s_id;
bool Lpt0_found, Lpt1_found; bool Lpt0_found, Lpt1_found;
int Lpt1[2], Lpt0[2]; int Lpt1[2], Lpt0[2];
@@ -93,7 +93,7 @@ void get_corners();
int main(void) int main(void)
{ {
clock_init(SYSTEM_CLOCK_120M); // <20><>ʼ<EFBFBD><CABC>оƬʱ<C6AC><CAB1> <20><><EFBFBD><EFBFBD>Ƶ<EFBFBD><C6B5>Ϊ 120MHz clock_init(SYSTEM_CLOCK_120M); // <20><>ʼ<EFBFBD><CABC>оƬʱ<C6AC><CAB1> <20><><EFBFBD><EFBFBD>Ƶ<EFBFBD><C6B5>Ϊ 120MHz
debug_init(); // <20><><EFBFBD>ر<EFBFBD><D8B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڳ<EFBFBD>ʼ<EFBFBD><CABC>MPU ʱ<><CAB1> <20><><EFBFBD>Դ<EFBFBD><D4B4><EFBFBD> debug_init(); // <20><><EFBFBD>ر<EFBFBD><D8B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڳ<EFBFBD>ʼ<EFBFBD><CABC> MPU ʱ<><CAB1> <20><><EFBFBD>Դ<EFBFBD><D4B4><EFBFBD>
// mt9v03x_init(); // mt9v03x_init();
ips200_init(IPS200_TYPE_SPI); ips200_init(IPS200_TYPE_SPI);
by_gpio_init(); by_gpio_init();
@@ -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) {