btl143/QD4C-firmware
4
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request '陀螺仪的初级数据获取' (#8) from bmy/firmware_violet_zf:master into master

Browse Source 
Reviewed-on: http://git.isthmus.tk:441/btl143/firmware_violet_zf/pulls/8
This commit is contained in:
bmy
2023-12-22 22:43:40 +08:00
parent 8feb4bab66 20666b0c75
commit c5e64f3cb0
7 changed files with 284 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files

171
app/by_imu.c Normal file
View File

@@ -0,0 +1,171 @@
#include "by_imu.h"
#include "zf_common_headfile.h"
#include <math.h>
#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;
}
}

46
app/by_imu.h Normal file
View File

@@ -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

30
app/by_pt_button.c
View File

@@ -1,6 +1,8 @@
#include "by_pt_button.h" #include "by_pt_button.h"
#include "zf_common_headfile.h" #include "zf_common_headfile.h"
#include "by_imu.h"
uint8_t potate_button; uint8_t potate_button;
void by_gpio_init(void) void by_gpio_init(void)
{ {
gpio_init(E10, GPI, GPIO_HIGH, GPI_PULL_UP); 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) void by_ips_show(void)
{ {
ips114_show_string(0, 0, "button statu:"); ips200_show_string(0, 0, "button statu:");
ips114_show_uint(104, 0, by_get_pb_statu(), 1); // 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));
} }

4
app/by_pt_button.h
View File

@@ -8,8 +8,10 @@
#define POTATE_BUTTOM_BACKWARD 3 #define POTATE_BUTTOM_BACKWARD 3
extern uint8_t potate_button; extern uint8_t potate_button;
extern void by_exit_init(void); extern void by_exit_init(void);
extern void by_gpio_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); extern void by_ips_show(void);
#endif #endif

7
app/isr.c
View File

@@ -195,14 +195,14 @@ void EXTI9_5_IRQHandler(void)
EXTI_ClearITPendingBit(EXTI_Line8); EXTI_ClearITPendingBit(EXTI_Line8);
} }
if (SET == EXTI_GetITStatus(EXTI_Line9)) { if (SET == EXTI_GetITStatus(EXTI_Line9)) {
EXTI_ClearITPendingBit(EXTI_Line9);
if (SET == gpio_get_level(E10)) { if (SET == gpio_get_level(E10)) {
potate_button = POTATE_BUTTOM_BACKWARD; potate_button = POTATE_BUTTOM_BACKWARD;
} else { } else {
potate_button = POTATE_BUTTOM_FOREWARD; potate_button = POTATE_BUTTOM_FOREWARD;
} }
EXTI_ClearITPendingBit(EXTI_Line9);
} }
} }
@@ -217,12 +217,12 @@ void EXTI15_10_IRQHandler(void)
EXTI_ClearITPendingBit(EXTI_Line10); EXTI_ClearITPendingBit(EXTI_Line10);
} }
if (SET == EXTI_GetITStatus(EXTI_Line11)) { if (SET == EXTI_GetITStatus(EXTI_Line11)) {
EXTI_ClearITPendingBit(EXTI_Line11);
system_delay_us(200); system_delay_us(200);
if (SET == !gpio_get_level(E11)) { if (SET == !gpio_get_level(E11)) {
potate_button = POTATE_BUTTOM_PRESS; potate_button = POTATE_BUTTOM_PRESS;
} }
EXTI_ClearITPendingBit(EXTI_Line11);
} }
if (SET == EXTI_GetITStatus(EXTI_Line12)) { if (SET == EXTI_GetITStatus(EXTI_Line12)) {
EXTI_ClearITPendingBit(EXTI_Line12); EXTI_ClearITPendingBit(EXTI_Line12);
@@ -289,6 +289,7 @@ void TIM5_IRQHandler(void)
void TIM6_IRQHandler(void) void TIM6_IRQHandler(void)
{ {
if (TIM_GetITStatus(TIM6, TIM_IT_Update) != RESET) { if (TIM_GetITStatus(TIM6, TIM_IT_Update) != RESET) {
ICM_getEulerianAngles();
TIM_ClearITPendingBit(TIM6, TIM_IT_Update); TIM_ClearITPendingBit(TIM6, TIM_IT_Update);
} }
} }

52
app/main.c
View File

@@ -37,9 +37,10 @@
#include "cw_servo.h" #include "cw_servo.h"
#include "by_pt_button.h" #include "by_pt_button.h"
#include "by_fan_control.h" #include "by_fan_control.h"
#include "by_imu.h"
uint8 (*Img_Gray)[MT9V03X_W]; // <20><><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD> MT9V03X_W <20>е<EFBFBD> uint8 <20><><EFBFBD>͵Ķ<CDB5>ά<EFBFBD><CEAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8> uint8 (*Img_Gray)[MT9V03X_W]; // <20><><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD> MT9V03X_W <20>е<EFBFBD> uint8 <20><><EFBFBD>͵Ķ<CDB5>ά<EFBFBD><CEAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>
//uint8 *mt9v03x_image_copy[0]; // <20><><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8> uint8 <20><><EFBFBD>͵<EFBFBD>һά<D2BB><CEAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8> // uint8 *mt9v03x_image_copy[0]; // <20><><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8> uint8 <20><><EFBFBD>͵<EFBFBD>һά<D2BB><CEAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>
sint32 pts_left[PT_MAXLEN][2], pts_right[PT_MAXLEN][2]; sint32 pts_left[PT_MAXLEN][2], pts_right[PT_MAXLEN][2];
sint32 pts_left_count, pts_right_count; sint32 pts_left_count, pts_right_count;
float32 pts_inv_l[PT_MAXLEN][2], pts_inv_r[PT_MAXLEN][2]; 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]; 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>+<2B>Ǽ<EFBFBD><C7BC><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>
//<2F><><EFBFBD>ұ<EFBFBD><D2B1>߾ֲ<DFBE><D6B2>Ƕȱ仯<C8B1><E4BBAF>+<2B>Ǽ<EFBFBD><C7BC><EFBFBD>ֵ<EFBFBD><D6B5><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;
@@ -66,7 +66,7 @@ int angle_left_num, angle_right_num;
// L<>ǵ<EFBFBD> // L<>ǵ<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];
int Lpt_in0_rpts0s_id, Lpt_in1_rpts1s_id; int Lpt_in0_rpts0s_id, Lpt_in1_rpts1s_id;
bool Lpt_in0_found, Lpt_in1_found; bool Lpt_in0_found, Lpt_in1_found;
@@ -85,44 +85,45 @@ float aim_distance;
enum track_type_e track_type = TRACK_RIGHT; enum track_type_e track_type = TRACK_RIGHT;
int frame_count = 0; int frame_count = 0;
void img_processing(); void img_processing();
void get_corners(); 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();
ips114_init(); ips200_init(IPS200_TYPE_SPI);
by_gpio_init(); by_gpio_init();
by_exit_init(); by_exit_init();
by_pwm_init(); by_pwm_init();
cw_servo_init(); cw_servo_init();
while (imu660ra_init())
;
gyroOffset_init();
pit_ms_init(TIM6_PIT, 1);
while (1) { while (1) {
//while (frame_count < 20) { // while (frame_count < 20) {
// if (mt9v03x_finish_flag) { // if (mt9v03x_finish_flag) {
// memcpy(mt9v03x_image_copy[0], mt9v03x_image[0], (sizeof(mt9v03x_image_copy) / sizeof(uint8_t))); // 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); // 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); // //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); // ips114_show_gray_image(0, 0, mt9v03x_image_copy[0], MT9V03X_W, MT9V03X_H, MT9V03X_W, MT9V03X_H, 0);
// mt9v03x_finish_flag = 0; // mt9v03x_finish_flag = 0;
// frame_count++; // frame_count++;
// } // }
//} //}
if (mt9v03x_finish_flag) { if (mt9v03x_finish_flag) {
//ips114_show_gray_image(0, 0, mt9v03x_image[0], 188, 120, 188, 120,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))); memcpy(mt9v03x_image_copy[0], mt9v03x_image[0], (sizeof(mt9v03x_image_copy) / sizeof(uint8_t)));
//Img_Gray = mt9v03x_image; // Img_Gray = mt9v03x_image;
//mt9v03x_image_copy[0] = Img_Gray[0]; // mt9v03x_image_copy[0] = Img_Gray[0];
mt9v03x_finish_flag = 0; mt9v03x_finish_flag = 0;
state_type = COMMON_STATE; state_type = COMMON_STATE;
img_processing(); img_processing();
@@ -137,10 +138,9 @@ int main(void)
ElementRun(); ElementRun();
MidLineTrack(); MidLineTrack();
} }
// by_imu_data_get();
by_ips_show();
system_delay_ms(200);
} }
} }

8
libraries/zf_device/zf_device_ips200.h
View File

@@ -85,10 +85,10 @@
#endif #endif
// 如果使用的是单排排针的两寸屏幕 SPI 驱动控制引脚 可以修改 // 如果使用的是单排排针的两寸屏幕 SPI 驱动控制引脚 可以修改
#define IPS200_RST_PIN_SPI (B7 ) // 液晶复位引脚定义 #define IPS200_RST_PIN_SPI (D8 ) // 液晶复位引脚定义
#define IPS200_DC_PIN_SPI (D7 ) // 液晶命令位引脚定义 #define IPS200_DC_PIN_SPI (D9 ) // 液晶命令位引脚定义
#define IPS200_CS_PIN_SPI (D4 ) #define IPS200_CS_PIN_SPI (D10 )
#define IPS200_BLk_PIN_SPI (D0 ) #define IPS200_BLk_PIN_SPI (D11 )
// --------------------单排两寸屏幕SPI接口引脚定义--------------------// // --------------------单排两寸屏幕SPI接口引脚定义--------------------//