diff --git a/app/by_imu.c b/app/by_imu.c new file mode 100644 index 0000000..f64bd09 --- /dev/null +++ b/app/by_imu.c @@ -0,0 +1,171 @@ +#include "by_imu.h" +#include "zf_common_headfile.h" +#include + +#define delta_T 0.001f // 1ms计算一次 + +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(); + + 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..7977aa2 --- /dev/null +++ b/app/by_imu.h @@ -0,0 +1,46 @@ +#ifndef _BY_IMU_H__ +#define _BY_IMU_H__ + +#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; + +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; + +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..eea70a9 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 e9e33be..fd646a1 100644 --- a/app/main.c +++ b/app/main.c @@ -37,9 +37,10 @@ #include "cw_servo.h" #include "by_pt_button.h" #include "by_fan_control.h" +#include "by_imu.h" uint8 (*Img_Gray)[MT9V03X_W]; // ָ MT9V03X_W е uint8 ͵Ķάָ -//uint8 *mt9v03x_image_copy[0]; // ָ uint8 ͵һάָ +// uint8 *mt9v03x_image_copy[0]; // ָ uint8 ͵һάָ sint32 pts_left[PT_MAXLEN][2], pts_right[PT_MAXLEN][2]; sint32 pts_left_count, pts_right_count; float32 pts_inv_l[PT_MAXLEN][2], pts_inv_r[PT_MAXLEN][2]; @@ -51,8 +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; @@ -66,7 +66,7 @@ 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]; +int Lpt1[2], Lpt0[2]; int Lpt_in0_rpts0s_id, Lpt_in1_rpts1s_id; bool Lpt_in0_found, Lpt_in1_found; @@ -85,44 +85,45 @@ float aim_distance; enum track_type_e track_type = TRACK_RIGHT; - int frame_count = 0; void img_processing(); void get_corners(); - int main(void) { clock_init(SYSTEM_CLOCK_120M); // ʼоƬʱ ƵΪ 120MHz debug_init(); // رڳʼMPU ʱ Դ - mt9v03x_init(); - ips114_init(); + // mt9v03x_init(); + ips200_init(IPS200_TYPE_SPI); by_gpio_init(); by_exit_init(); by_pwm_init(); cw_servo_init(); + while (imu660ra_init()) + ; + gyroOffset_init(); + pit_ms_init(TIM6_PIT, 1); while (1) { - //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++; - // } + // 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) { - //ips114_show_gray_image(0, 0, mt9v03x_image[0], 188, 120, 188, 120,0); - memcpy(mt9v03x_image_copy[0], mt9v03x_image[0],(sizeof(mt9v03x_image_copy)/sizeof(uint8_t))); - //Img_Gray = mt9v03x_image; - //mt9v03x_image_copy[0] = Img_Gray[0]; + // ips114_show_gray_image(0, 0, mt9v03x_image[0], 188, 120, 188, 120,0); + memcpy(mt9v03x_image_copy[0], mt9v03x_image[0], (sizeof(mt9v03x_image_copy) / sizeof(uint8_t))); + // Img_Gray = mt9v03x_image; + // mt9v03x_image_copy[0] = Img_Gray[0]; mt9v03x_finish_flag = 0; - state_type = COMMON_STATE; img_processing(); @@ -137,10 +138,9 @@ int main(void) ElementRun(); MidLineTrack(); - - } - - + // by_imu_data_get(); + by_ips_show(); + system_delay_ms(200); } } \ 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 8a4c04d..090b71a 100644 --- a/libraries/zf_device/zf_device_ips200.h +++ b/libraries/zf_device/zf_device_ips200.h @@ -85,10 +85,10 @@ #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 ) +#define IPS200_BLk_PIN_SPI (D11 ) // --------------------单排两寸屏幕SPI接口引脚定义--------------------//