From 815a9821a07f967c1dcc6e0636d82230e3dd55fe Mon Sep 17 00:00:00 2001 From: CaoWangrenbo Date: Sun, 24 Dec 2023 15:39:44 +0800 Subject: [PATCH] =?UTF-8?q?=E6=9B=B4=E6=96=B0=E9=99=80=E8=9E=BA=E4=BB=AA?= =?UTF-8?q?=E7=9B=B8=E5=85=B3=E8=AE=BE=E7=BD=AE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- app/by_imu.c | 69 ++++++++++++++++++++++++++++++---------------- app/by_imu.h | 3 +- app/by_pt_button.c | 2 +- app/main.c | 18 ++++++------ 4 files changed, 58 insertions(+), 34 deletions(-) diff --git a/app/by_imu.c b/app/by_imu.c index f64bd09..bad29e9 100644 --- a/app/by_imu.c +++ b/app/by_imu.c @@ -2,7 +2,7 @@ #include "zf_common_headfile.h" #include -#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; diff --git a/app/by_imu.h b/app/by_imu.h index 7977aa2..63b681b 100644 --- a/app/by_imu.h +++ b/app/by_imu.h @@ -1,7 +1,7 @@ #ifndef _BY_IMU_H__ #define _BY_IMU_H__ -#include "stdio.h" +#include #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); diff --git a/app/by_pt_button.c b/app/by_pt_button.c index eea70a9..549478d 100644 --- a/app/by_pt_button.c +++ b/app/by_pt_button.c @@ -45,7 +45,7 @@ void by_ips_show(void) ips200_show_float(46, 32, eulerAngle.pitch, 3, 2); ips200_show_float(46, 48, eulerAngle.roll, 3, 2); ips200_show_float(46, 64, eulerAngle.yaw, 3, 2); - // ips200_show_float(46 , 32, icm_data.gyro_x, 2, 2); + // ips200_show_float(46 , 32, icm_data.gyro_x, 2, 2); // ips200_show_float(106, 32, icm_data.gyro_y, 2, 2); // ips200_show_float(166, 32, icm_data.gyro_z, 2, 2); ips200_show_float(46, 80, imu660ra_temperature, 2, 2); diff --git a/app/main.c b/app/main.c index fd646a1..3b0d909 100644 --- a/app/main.c +++ b/app/main.c @@ -2,11 +2,11 @@ * CH32V307VCT6 Opensourec Library CH32V307VCT6 Դ⣩һڹٷ SDK ӿڵĵԴ * Copyright (c) 2022 SEEKFREE ɿƼ * - * ļCH32V307VCT6 Դһ + * ļ CH32V307VCT6 Դһ * * CH32V307VCT6 Դ - * Ըᷢ GPLGNU General Public License GNUͨù֤ - * GPL ĵ3棨 GPL3.0ѡģκκİ汾·/޸ + * Ըᷢ GPLGNU General Public License GNU ͨù֤ + * GPL ĵ 3 棨 GPL3.0ѡģκκİ汾·/޸ * * Դķϣܷãδκεı֤ * ûԻʺض;ı֤ @@ -30,7 +30,7 @@ * * ޸ļ¼ * ע - * 2022-09-15 W first version + * 2022-09-15 W first version ********************************************************************************************************************/ #include "zf_common_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]; sint32 mid_left_count, mid_right_count; -// ұ߾ֲǶȱ仯+Ǽֵ +// ұ߾ֲǶȱ仯 + Ǽֵ float angle_new_left[PT_MAXLEN]; float angle_new_right[PT_MAXLEN]; int angle_new_left_num, angle_new_right_num; @@ -63,7 +63,7 @@ float angle_left[PT_MAXLEN]; float angle_right[PT_MAXLEN]; int angle_left_num, angle_right_num; -// Lǵ +// L ǵ int Lpt0_rpts0s_id, Lpt1_rpts1s_id; bool Lpt0_found, Lpt1_found; int Lpt1[2], Lpt0[2]; @@ -93,7 +93,7 @@ void get_corners(); int main(void) { clock_init(SYSTEM_CLOCK_120M); // ʼоƬʱ ƵΪ 120MHz - debug_init(); // رڳʼMPU ʱ Դ + debug_init(); // رڳʼ MPU ʱ Դ // mt9v03x_init(); ips200_init(IPS200_TYPE_SPI); by_gpio_init(); @@ -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) {