|
|
分享一套基于32的程序，參考了匿名等程序
0.png (49.46 KB, 下載次數(shù): 41)
下載附件
2017-5-15 16:16 上傳
編譯,下載,運(yùn)行,連接飛控串口和FTDI串口,串口波特率500K,上位機(jī)打開高級收碼,在上位機(jī)飛控狀態(tài)標(biāo)簽可以看到變化的傳感器數(shù)據(jù),3D顯示可以跟隨roll和pitch的變化而變化,因為沒有寫yaw的上傳,所以yaw保持零度...此時打開飛控波形按鈕,打開波形顯示頁面,并打開相應(yīng)波形開關(guān),1到3為加速度,4到6為陀螺儀,10和11為roll和pitch,就可以看到變化的波形.
單片機(jī)源程序如下:
- #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; //四元數(shù)計算出的角度
- int16_t ACC_X_BUF[FILTER_NUM],ACC_Y_BUF[FILTER_NUM],ACC_Z_BUF[FILTER_NUM]; //加速度滑動窗口濾波數(shù)組
- 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;//更新滑動窗口數(shù)組
- 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的執(zhí)行周期
- 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轉(zhuǎn)成弧度
- }
- ////////////////////////////////////////////////////////////////////////////////
- #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數(shù)據(jù)歸一化
- 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; //對誤差進(jìn)行積分
- eyInt = eyInt + ey * Ki;
- ezInt = ezInt + ez * Ki;
- // adjusted gyroscope measurements
- gx = gx + Kp*ex + exInt; //將誤差PI后補(bǔ)償?shù)酵勇輧x，即補(bǔ)償零點(diǎn)漂移
- gy = gy + Kp*ey + eyInt;
- gz = gz + Kp*ez + ezInt; //這里的gz由于沒有觀測者進(jìn)行矯正會產(chǎn)生漂移，表現(xiàn)出來的就是積分自增或自減
- // 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, 下載次數(shù): 63)
2022-12-15 18:10 上傳
點(diǎn)擊文件名下載附件
下載積分: 黑幣 -5
|
|
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
