diff --git a/app/by_imu.c b/app/by_imu.c index 11f945a..f64bd09 100644 --- a/app/by_imu.c +++ b/app/by_imu.c @@ -1,26 +1,171 @@ #include "by_imu.h" #include "zf_common_headfile.h" +#include -float imu_acc_x; -float imu_acc_y; -float imu_acc_z; -float imu_gyro_x; -float imu_gyro_y; -float imu_gyro_z; +#define delta_T 0.001f // 1ms计算一次 -void by_imu_data_get(void) +float I_ex, I_ey, I_ez; // 误差积分 + +quater_param_t Q_info = {1, 0, 0}; // 全局四元数 +euler_param_t eulerAngle; // 欧拉角 + +icm_param_t icm_data; +gyro_param_t GyroOffset; + +bool GyroOffset_init = 0; + +float param_Kp = 0.17; // 加速度计的收敛速率比例增益 0.17 +float param_Ki = 0.008; // 陀螺仪收敛速率的积分增益 0.004 + +float fast_sqrt(float x) { + float halfx = 0.5f * x; + float y = x; + long i = *(long *)&y; + i = 0x5f3759df - (i >> 1); + y = *(float *)&i; + y = y * (1.5f - (halfx * y * y)); + return y; +} + +void gyroOffset_init(void) /////////陀螺仪零飘初始化 +{ + GyroOffset.Xdata = 0; + GyroOffset.Ydata = 0; + GyroOffset.Zdata = 0; + for (uint16_t i = 0; i < 500; ++i) { + imu660ra_get_acc(); + imu660ra_get_gyro(); + GyroOffset.Xdata += imu660ra_gyro_x; + GyroOffset.Ydata += imu660ra_gyro_y; + GyroOffset.Zdata += imu660ra_gyro_z; + system_delay_ms(1); + } + + GyroOffset.Xdata /= 500.0f; + GyroOffset.Ydata /= 500.0f; + GyroOffset.Zdata /= 500.0f; + + 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); + + 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; +} + +// 互补滤波 +void ICM_AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az) +{ + float halfT = 0.5 * delta_T; + float vx, vy, vz; // 当前的机体坐标系上的重力单位向量 + float ex, ey, ez; // 四元数计算值与加速度计测量值的误差 + float q0 = Q_info.q0; + float q1 = Q_info.q1; + float q2 = Q_info.q2; + float q3 = Q_info.q3; + float q0q0 = q0 * q0; + float q0q1 = q0 * q1; + float q0q2 = q0 * q2; + float q0q3 = q0 * q3; + float q1q1 = q1 * q1; + float q1q2 = q1 * q2; + float q1q3 = q1 * q3; + float q2q2 = q2 * q2; + float q2q3 = q2 * q3; + float q3q3 = q3 * q3; + // float delta_2 = 0; + + // 对加速度数据进行归一化 得到单位加速度 + float norm = fast_sqrt(ax * ax + ay * ay + az * az); + ax = ax * norm; + ay = ay * norm; + az = az * norm; + + // 根据当前四元数的姿态值来估算出各重力分量。用于和加速计实际测量出来的各重力分量进行对比，从而实现对四轴姿态的修正 + 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 = (q0*q0-0.5f+q3 * q3) * 2; + + // 叉积来计算估算的重力和实际测量的重力这两个重力向量之间的误差。 + ex = ay * vz - az * vy; + ey = az * vx - ax * vz; + ez = ax * vy - ay * vx; + + // 用叉乘误差来做PI修正陀螺零偏， + // 通过调节 param_Kp，param_Ki 两个参数， + // 可以控制加速度计修正陀螺仪积分姿态的速度。 + I_ex += halfT * ex; // integral error scaled by Ki + I_ey += halfT * ey; + I_ez += halfT * ez; + + gx = gx + param_Kp * ex + param_Ki * I_ex; + gy = gy + param_Kp * ey + param_Ki * I_ey; + gz = gz + param_Kp * ez + param_Ki * I_ez; + + /*数据修正完成，下面是四元数微分方程*/ + + // 四元数微分方程，其中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; + q3 = q3 + (q0 * gz + q1 * gy - q2 * gx) * halfT; + // delta_2=(2*halfT*gx)*(2*halfT*gx)+(2*halfT*gy)*(2*halfT*gy)+(2*halfT*gz)*(2*halfT*gz); + // 整合四元数率 四元数微分方程 四元数更新算法，二阶毕卡法 + // q0 = (1-delta_2/8)*q0 + (-q1*gx - q2*gy - q3*gz)*halfT; + // q1 = (1-delta_2/8)*q1 + (q0*gx + q2*gz - q3*gy)*halfT; + // q2 = (1-delta_2/8)*q2 + (q0*gy - q1*gz + q3*gx)*halfT; + // q3 = (1-delta_2/8)*q3 + (q0*gz + q1*gy - q2*gx)*halfT + + // normalise quaternion + norm = fast_sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + Q_info.q0 = q0 * norm; + Q_info.q1 = q1 * norm; + Q_info.q2 = q2 * norm; + Q_info.q3 = q3 * norm; +} +void ICM_getEulerianAngles(void) +{ + + // 采集陀螺仪数据 imu660ra_get_acc(); imu660ra_get_gyro(); imu660ra_get_temperature(); - imu_acc_x = imu660ra_acc_transition(imu660ra_acc_x); - imu_acc_y = imu660ra_acc_transition(imu660ra_acc_y); - imu_acc_z = imu660ra_acc_transition(imu660ra_acc_z); + ICM_getValues(); + ICM_AHRSupdate(icm_data.gyro_x, icm_data.gyro_y, icm_data.gyro_z, icm_data.acc_x, icm_data.acc_y, icm_data.acc_z); + float q0 = Q_info.q0; + float q1 = Q_info.q1; + float q2 = Q_info.q2; + float q3 = Q_info.q3; - imu_gyro_x = imu660ra_gyro_transition(imu660ra_gyro_x); - imu_gyro_y = imu660ra_gyro_transition(imu660ra_gyro_y); - imu_gyro_z = imu660ra_gyro_transition(imu660ra_gyro_z); + // 四元数计算欧拉角 + eulerAngle.pitch = asin(-2 * q1 * q3 + 2 * q0 * q2) * 180 / M_PI; // pitch + eulerAngle.roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 180 / M_PI; // roll + eulerAngle.yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2 * q2 - 2 * q3 * q3 + 1) * 180 / M_PI; // yaw - + /* 姿态限制*/ + if (eulerAngle.roll > 90 || eulerAngle.roll < -90) { + if (eulerAngle.pitch > 0) { + eulerAngle.pitch = 180 - eulerAngle.pitch; + } + if (eulerAngle.pitch < 0) { + eulerAngle.pitch = -(180 + eulerAngle.pitch); + } + } + + if (eulerAngle.yaw > 360) { + eulerAngle.yaw -= 360; + } else if (eulerAngle.yaw < 0) { + eulerAngle.yaw += 360; + } } diff --git a/app/by_imu.h b/app/by_imu.h index 0e81a20..7977aa2 100644 --- a/app/by_imu.h +++ b/app/by_imu.h @@ -3,13 +3,44 @@ #include "stdio.h" #include "ch32v30x.h" +typedef struct { + float gyro_x; + float gyro_y; + float gyro_z; + float acc_x; + float acc_y; + float acc_z; +} icm_param_t; -extern float imu_acc_x; -extern float imu_acc_y; -extern float imu_acc_z; -extern float imu_gyro_x; -extern float imu_gyro_y; -extern float imu_gyro_z; +typedef struct { + float q0; + float q1; + float q2; + float q3; +} quater_param_t; -extern void by_imu_data_get(void); +typedef struct { + float pitch; //俯仰角 + float roll; //偏航角 + float yaw; //翻滚角 +} euler_param_t; + +typedef struct { + float Xdata; + float Ydata; + float Zdata; +} gyro_param_t; + +extern icm_param_t icm_data; +extern euler_param_t eulerAngle; + +void gyroOffset_init(void); + +float fast_sqrt(float x); + +void ICM_AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az); + +void ICM_getValues(); + +void ICM_getEulerianAngles(void); #endif \ No newline at end of file diff --git a/app/by_pt_button.c b/app/by_pt_button.c index 97f5ab1..eea70a9 100644 --- a/app/by_pt_button.c +++ b/app/by_pt_button.c @@ -42,13 +42,12 @@ void by_ips_show(void) } // 按钮 ips200_show_string(0, 16, "imu:"); - ips200_show_float(46, 16, imu_acc_x, 2, 2); - ips200_show_float(106, 16, imu_acc_y, 2, 2); - ips200_show_float(166, 16, imu_acc_z, 2, 2); - ips200_show_float(46, 32, imu_gyro_x, 2, 2); - ips200_show_float(106, 32, imu_gyro_y, 2, 2); - ips200_show_float(166, 32, imu_gyro_z, 2, 2); - ips200_show_float(46, 48, imu660ra_temperature, 2, 2); - printf("%d,%d,%d\n",(int16_t)(imu_gyro_x*10),(int16_t)(imu_gyro_y*10),(int16_t)(imu_gyro_z*10)); - + 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(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); + // printf("%d,%d,%d\n", (int16_t)(icm_data.gyro_x * 10), (int16_t)(icm_data.gyro_y * 10), (int16_t)(icm_data.gyro_z * 10)); } diff --git a/app/isr.c b/app/isr.c index e502d42..ce1e22e 100644 --- a/app/isr.c +++ b/app/isr.c @@ -195,15 +195,14 @@ void EXTI9_5_IRQHandler(void) EXTI_ClearITPendingBit(EXTI_Line8); } if (SET == EXTI_GetITStatus(EXTI_Line9)) { - EXTI_ClearITPendingBit(EXTI_Line9); + if (SET == gpio_get_level(E10)) { potate_button = POTATE_BUTTOM_BACKWARD; - } else { potate_button = POTATE_BUTTOM_FOREWARD; - } + EXTI_ClearITPendingBit(EXTI_Line9); } } @@ -218,13 +217,12 @@ void EXTI15_10_IRQHandler(void) EXTI_ClearITPendingBit(EXTI_Line10); } if (SET == EXTI_GetITStatus(EXTI_Line11)) { - EXTI_ClearITPendingBit(EXTI_Line11); system_delay_us(200); if (SET == !gpio_get_level(E11)) { potate_button = POTATE_BUTTOM_PRESS; - } + EXTI_ClearITPendingBit(EXTI_Line11); } if (SET == EXTI_GetITStatus(EXTI_Line12)) { EXTI_ClearITPendingBit(EXTI_Line12); @@ -291,6 +289,7 @@ void TIM5_IRQHandler(void) void TIM6_IRQHandler(void) { if (TIM_GetITStatus(TIM6, TIM_IT_Update) != RESET) { + ICM_getEulerianAngles(); TIM_ClearITPendingBit(TIM6, TIM_IT_Update); } } diff --git a/app/main.c b/app/main.c index 58bca7f..fd646a1 100644 --- a/app/main.c +++ b/app/main.c @@ -102,6 +102,8 @@ int main(void) cw_servo_init(); while (imu660ra_init()) ; + gyroOffset_init(); + pit_ms_init(TIM6_PIT, 1); while (1) { // while (frame_count < 20) { @@ -137,7 +139,7 @@ int main(void) MidLineTrack(); } - by_imu_data_get(); + // by_imu_data_get(); by_ips_show(); system_delay_ms(200); }