diff --git a/app/by_imu.c b/app/by_imu.c new file mode 100644 index 0000000..bad29e9 --- /dev/null +++ b/app/by_imu.c @@ -0,0 +1,192 @@ +#include "by_imu.h" +#include "zf_common_headfile.h" +#include + +#define delta_T 0.002f // 2ms 计算一次 + +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.004; // 陀螺仪收敛速率的积分增益 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) /////////陀螺仪零飘初始化 +{ + 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 < 5000; i++) { + imu660ra_get_gyro(); + + 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 /= 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.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.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); +} + +// 互补滤波 +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(); + + 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; + + // 四元数计算欧拉角 + 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 new file mode 100644 index 0000000..63b681b --- /dev/null +++ b/app/by_imu.h @@ -0,0 +1,47 @@ +#ifndef _BY_IMU_H__ +#define _BY_IMU_H__ + +#include +#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; + +typedef struct { + float q0; + float q1; + float q2; + float q3; +} quater_param_t; + +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; +extern gyro_param_t GyroOffset; + +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 fc0a5ce..549478d 100644 --- a/app/by_pt_button.c +++ b/app/by_pt_button.c @@ -1,6 +1,8 @@ #include "by_pt_button.h" #include "zf_common_headfile.h" +#include "by_imu.h" uint8_t potate_button; + void by_gpio_init(void) { gpio_init(E10, GPI, GPIO_HIGH, GPI_PULL_UP); @@ -21,7 +23,31 @@ uint8_t by_get_pb_statu(void) void by_ips_show(void) { - ips114_show_string(0, 0, "button statu:"); - ips114_show_uint(104, 0, by_get_pb_statu(), 1); - -} \ No newline at end of file + ips200_show_string(0, 0, "button statu:"); + // ips200_show_uint(104, 0, by_get_pb_statu(), 1); + switch (by_get_pb_statu()) { + case 1: + ips200_show_string(104, 0, "press"); + break; + case 2: + ips200_show_string(104, 0, "up "); + break; + case 3: + ips200_show_string(104, 0, "down "); + break; + + default: + ips200_show_string(104, 0, " "); + break; + } + // 按钮 + ips200_show_string(0, 16, "imu:"); + 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/by_pt_button.h b/app/by_pt_button.h index 4e0ded1..0a7202f 100644 --- a/app/by_pt_button.h +++ b/app/by_pt_button.h @@ -8,8 +8,10 @@ #define POTATE_BUTTOM_BACKWARD 3 extern uint8_t potate_button; + extern void by_exit_init(void); extern void by_gpio_init(void); -extern uint8_t by_get_pb_statu(void); +extern uint8_t by_get_statu(void); extern void by_ips_show(void); + #endif \ No newline at end of file diff --git a/app/isr.c b/app/isr.c index 33f643e..ce1e22e 100644 --- a/app/isr.c +++ b/app/isr.c @@ -195,14 +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); } } @@ -217,12 +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); @@ -289,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 18d6832..ae266ed 100644 --- a/app/main.c +++ b/app/main.c @@ -1,36 +1,36 @@ /********************************************************************************************************************* - * CH32V307VCT6 Opensourec Library CH32V307VCT6 Դ⣩һڹٷ SDK ӿڵĵԴ - * Copyright (c) 2022 SEEKFREE ɿƼ + * CH32V307VCT6 Opensourec Library ????CH32V307VCT6 ???????????????? SDK ?????????????? + * Copyright (c) 2022 SEEKFREE ????? * - * ļ CH32V307VCT6 Դһ + * ??????? CH32V307VCT6 ??????????? * - * CH32V307VCT6 Դ - * Ըᷢ GPLGNU General Public License GNU ͨù֤ - * GPL ĵ 3 棨 GPL3.0ѡģκκİ汾·/޸ + * CH32V307VCT6 ????? ????????? + * ?????????????????????????? GPL??GNU General Public License???? GNU ??��??????????????? + * ?? GPL ??? 3 ?��? GPL3.0????????????�ʦ�????��????��????/??????? * - * Դķϣܷãδκεı֤ - * ûԻʺض;ı֤ - * ϸμ GPL + * ????????????????????????????????��??????�ʦ�?? + * ????????????????????????????????? + * ?????????��GPL * - * ӦյԴͬʱյһ GPL ĸ - * ûУ + * ?????????????????????????? GPL ????? + * ?????��????? * - * ע - * Դʹ GPL3.0 Դ֤Э Ϊİ汾 - * Ӣİ libraries/doc ļµ GPL3_permission_statement.txt ļ - * ֤ libraries ļ ļµ LICENSE ļ - * ӭλʹò ޸ʱ뱣ɿƼİȨ + * ????????? + * ?????????? GPL3.0 ????????��? ?????????????????�� + * ?????????????? libraries/doc ???????? GPL3_permission_statement.txt ????? + * ??????????? libraries ??????? ???????????? LICENSE ??? + * ?????��?��?????????? ????????????????????????????????????????? * - * ļ main - * ˾ ɶɿƼ޹˾ - * 汾Ϣ 鿴 libraries/doc ļ version ļ 汾˵ - * MounRiver Studio V1.8.1 - * ƽ̨ CH32V307VCT6 - * https://seekfree.taobao.com/ + * ??????? main + * ??????? ?????????????? + * ?��?? ?? libraries/doc ??????? version ??? ?��?? + * ???????? MounRiver Studio V1.8.1 + * ?????? CH32V307VCT6 + * ???????? https://seekfree.taobao.com/ * - * ޸ļ¼ - * ע - * 2022-09-15 W first version + * ????? + * ???? ???? ??? + * 2022-09-15 ?? W first version ********************************************************************************************************************/ #include "zf_common_headfile.h" #include "gl_headfile.h" @@ -39,9 +39,10 @@ #include "cw_servo.h" #include "./page/cw_page.h" -uint8_t (*Img_Gray)[MT9V03X_W]; // ָ MT9V03X_W е uint8_t ͵Ķάָ -// uint8_t *mt9v03x_image_copy[0]; // ָ uint8_t ͵һάָ -int32_t pts_left[PT_MAXLEN][2], pts_right[PT_MAXLEN][2]; +uint8_t (*Img_Gray)[MT9V03X_W]; +// uint8_t *mt9v03x_image_copy[0]; +int32_t pts_left[PT_MAXLEN][2]; +int32_t pts_right[PT_MAXLEN][2]; int32_t pts_left_count, pts_right_count; float pts_inv_l[PT_MAXLEN][2], pts_inv_r[PT_MAXLEN][2]; int32_t pts_inv_l_count, pts_inv_r_count; @@ -52,18 +53,15 @@ int32_t pts_resample_left_count, pts_resample_right_count; float mid_left[PT_MAXLEN][2], mid_right[PT_MAXLEN][2]; int32_t 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; uint8_t mt9v03x_image_copy[MT9V03X_H][MT9V03X_W]; -// ұ߾ֲǶȱ仯 float angle_left[PT_MAXLEN]; float angle_right[PT_MAXLEN]; int angle_left_num, angle_right_num; -// L ǵ int Lpt0_rpts0s_id, Lpt1_rpts1s_id; bool Lpt0_found, Lpt1_found; int Lpt1[2], Lpt0[2]; @@ -72,13 +70,10 @@ int Lpt_in0_rpts0s_id, Lpt_in1_rpts1s_id; bool Lpt_in0_found, Lpt_in1_found; int Lpt_in1[2], Lpt_in0[2]; -// ֱ bool is_straight0, is_straight1; -// bool is_turn0, is_turn1; -// һ float rptsn[PT_MAXLEN][2]; int rptsn_num; float aim_distance; @@ -92,36 +87,24 @@ void get_corners(); int main(void) { - clock_init(SYSTEM_CLOCK_120M); // ʼоƬʱ ƵΪ 120MHz - debug_init(); // رڳʼ MPU ʱ Դ + clock_init(SYSTEM_CLOCK_120M); + debug_init(); mt9v03x_init(); ips200_init(IPS200_TYPE_SPI); by_gpio_init(); by_exit_init(); by_pwm_init(); cw_servo_init(); + // while (imu660ra_init()) + // ; Page_Init(); while (1) { Page_Run(); - // while (frame_count < 20) { - // if (mt9v03x_finish_flag) { - // memcpy(mt9v03x_image_copy[0], mt9v03x_image[0], (sizeof(mt9v03x_image_copy) / sizeof(uint8_t))); - // adaptiveThreshold((uint8_t*)mt9v03x_image_copy, (uint8_t*)mt9v03x_image_copy, 188, 120, 7, 8); - // //threshold((uint8_t*)mt9v03x_image_copy, (uint8_t*)mt9v03x_image_copy, MT9V03X_W, MT9V03X_H, 110); - // ips114_show_gray_image(0, 0, mt9v03x_image_copy[0], MT9V03X_W, MT9V03X_H, MT9V03X_W, MT9V03X_H, 0); - // mt9v03x_finish_flag = 0; - // frame_count++; - // } - //} - - /************************ ͼغ ***************************/ + if (mt9v03x_finish_flag) { memcpy(mt9v03x_image_copy[0], mt9v03x_image[0], (sizeof(mt9v03x_image_copy) / sizeof(uint8_t))); - // ips114_show_gray_image(0, 0, mt9v03x_image[0], 188, 120, 188, 120,0); - // Img_Gray = mt9v03x_image; - // mt9v03x_image_copy[0] = Img_Gray[0]; mt9v03x_finish_flag = 0; state_type = COMMON_STATE; @@ -133,6 +116,5 @@ int main(void) ElementRun(); MidLineTrack(); } - /************************ ͼغ ***************************/ } } \ No newline at end of file diff --git a/libraries/zf_device/zf_device_ips200.h b/libraries/zf_device/zf_device_ips200.h index 3ff2843..8e73b41 100644 --- a/libraries/zf_device/zf_device_ips200.h +++ b/libraries/zf_device/zf_device_ips200.h @@ -85,11 +85,6 @@ #endif // 如果使用的是单排排针的两寸屏幕 SPI 驱动控制引脚 可以修改 -// #define IPS200_RST_PIN_SPI (B7 ) // 液晶复位引脚定义 -// #define IPS200_DC_PIN_SPI (D7 ) // 液晶命令位引脚定义 -// #define IPS200_CS_PIN_SPI (D4 ) -// #define IPS200_BLk_PIN_SPI (D0 ) - #define IPS200_RST_PIN_SPI (D8 ) // 液晶复位引脚定义 #define IPS200_DC_PIN_SPI (D9 ) // 液晶命令位引脚定义 #define IPS200_CS_PIN_SPI (D10 )