分享一套基于32的程序，参考了匿名等程序
编译,下载,运行,连接飞控串口和FTDI串口,串口波特率500K,上位机打开高级收码,在上位机飞控状态标签可以看到变化的传感器数据,3D显示可以跟随roll和pitch的变化而变化,因为没有写yaw的上传,所以yaw保持零度...此时打开飞控波形按钮,打开波形显示页面,并打开相应波形开关,1到3为加速度,4到6为陀螺仪,10和11为roll和pitch,就可以看到变化的波形.
单片机源程序如下:
- #include "imu.h"
- #include "bsp/MPU6050.h"
- #include "math.h"
- #define RtA 57.324841 //弧度到角度
- #define AtR 0.0174533 //度到角度
- #define Acc_G 0.0011963 //加速度变成G
- #define Gyro_G 0.0152672 //角速度变成度
- #define Gyro_Gr 0.0002663
- #define FILTER_NUM 20
- S_INT16_XYZ ACC_AVG; //平均值滤波后的ACC
- S_FLOAT_XYZ GYRO_I; //陀螺仪积分
- S_FLOAT_XYZ EXP_ANGLE; //期望角度
- S_FLOAT_XYZ DIF_ANGLE; //期望角度与实际角度差
- S_FLOAT_XYZ Q_ANGLE; //四元数计算出的角度
- int16_t ACC_X_BUF[FILTER_NUM],ACC_Y_BUF[FILTER_NUM],ACC_Z_BUF[FILTER_NUM]; //加速度滑动窗口滤波数组
- void Prepare_Data(void)
- {
- static uint8_t filter_cnt=0;
- int32_t temp1=0,temp2=0,temp3=0;
- uint8_t i;
-
- MPU6050_Read();
- MPU6050_Dataanl();
-
- ACC_X_BUF[filter_cnt] = MPU6050_ACC_LAST.X;//更新滑动窗口数组
- ACC_Y_BUF[filter_cnt] = MPU6050_ACC_LAST.Y;
- ACC_Z_BUF[filter_cnt] = MPU6050_ACC_LAST.Z;
- for(i=0;i<FILTER_NUM;i++)
- {
- temp1 += ACC_X_BUF[i];
- temp2 += ACC_Y_BUF[i];
- temp3 += ACC_Z_BUF[i];
- }
- ACC_AVG.X = temp1 / FILTER_NUM;
- ACC_AVG.Y = temp2 / FILTER_NUM;
- ACC_AVG.Z = temp3 / FILTER_NUM;
- filter_cnt++;
- if(filter_cnt==FILTER_NUM) filter_cnt=0;
-
- GYRO_I.X += MPU6050_GYRO_LAST.X*Gyro_G*0.0001;//0.0001是时间间隔,两次prepare的执行周期
- GYRO_I.Y += MPU6050_GYRO_LAST.Y*Gyro_G*0.0001;
- GYRO_I.Z += MPU6050_GYRO_LAST.Z*Gyro_G*0.0001;
- }
- void Get_Attitude(void)
- {
- IMUupdate(MPU6050_GYRO_LAST.X*Gyro_Gr,
- MPU6050_GYRO_LAST.Y*Gyro_Gr,
- MPU6050_GYRO_LAST.Z*Gyro_Gr,
- ACC_AVG.X,ACC_AVG.Y,ACC_AVG.Z); //*0.0174转成弧度
- }
- ////////////////////////////////////////////////////////////////////////////////
- #define Kp 10.0f // proportional gain governs rate of convergence to accelerometer/magnetometer
- #define Ki 0.008f // integral gain governs rate of convergence of gyroscope biases
- #define halfT 0.001f // half the sample period采样周期的一半
- float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // quaternion elements representing the estimated orientation
- float exInt = 0, eyInt = 0, ezInt = 0; // scaled integral error
- void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
- {
- float norm;
- // float hx, hy, hz, bx, bz;
- float vx, vy, vz;// wx, wy, wz;
- float ex, ey, ez;
- // 先把这些用得到的值算好
- 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;
-
- if(ax*ay*az==0)
- return;
-
- norm = sqrt(ax*ax + ay*ay + az*az); //acc数据归一化
- ax = ax /norm;
- ay = ay / norm;
- az = az / norm;
- // estimated direction of gravity and flux (v and w) 估计重力方向和流量/变迁
- vx = 2*(q1q3 - q0q2); //四元素中xyz的表示
- vy = 2*(q0q1 + q2q3);
- vz = q0q0 - q1q1 - q2q2 + q3q3 ;
- // error is sum of cross product between reference direction of fields and direction measured by sensors
- ex = (ay*vz - az*vy) ; //向量外积在相减得到差分就是误差
- ey = (az*vx - ax*vz) ;
- ez = (ax*vy - ay*vx) ;
- exInt = exInt + ex * Ki; //对误差进行积分
- eyInt = eyInt + ey * Ki;
- ezInt = ezInt + ez * Ki;
- // adjusted gyroscope measurements
- gx = gx + Kp*ex + exInt; //将误差PI后补偿到陀螺仪，即补偿零点漂移
- gy = gy + Kp*ey + eyInt;
- gz = gz + Kp*ez + ezInt; //这里的gz由于没有观测者进行矫正会产生漂移，表现出来的就是积分自增或自减
- // integrate quaternion rate and normalise //四元素的微分方程
- 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;
- ……………………
- …………限于本文篇幅 余下代码请从51黑下载附件…………
所有资料51hei提供下载:
飞控程序.7z
(259.85 KB, 下载次数: 63)
|
