|
|
楼主 |
发表于 2013-4-18 12:55:54
|
显示全部楼层
cai_mouse 发表于 2013-4-15 23:23 
楼主，能详细说一下飞控算法处理吗？
//=====================================================================================================
// IMU.c
// 四元素角速度更新
// 通过重力在B坐标系分量误差 叉乘得到B系各个轴分量
// KP+KI跟踪重力标定
// 弧度近似SIN
//=====================================================================================================
#include "IMU.h"
#include <math.h>
//----------------------------------------------------------------------------------------------------
// Definitions
#define Kp 2.0f // 比例增益支配率收敛到加速度计/磁强计
#define Ki 0.005f // 积分增益支配率的陀螺仪偏见的衔接
#define halfT 0.5f // 采样周期的一半
//---------------------------------------------------------------------------------------------------
// 变量定义
float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // 四元数的元素，代表估计方向
float exInt = 0, eyInt = 0, ezInt = 0; // 按比例缩小积分误差
//互补滤波
//时间常数 t=a/(1-a)*dt a=t/(t+dt) t 截至频率 dt计算间隔时间
#define kfa 0.98
#define kfan 1.0-kfa
//ang= kfa*ang+kfgn*acc;
#define kfg 0.80
#define kfgn 1.0-kfg
//ang= kfa*g+kfgn*acc;
//====================================================================================================
// Function
//====================================================================================================
//gx 采样时间弧度增量
//加数度为3轴采样数据，做归一化
//输出为4元数
void IMUupdate(float gxi, float gyi, float gzi, float axi, float ayi, float azi) {
float norm;
float vx, vy, vz;
float ex, ey, ez;
float gx,gy,gz,ax,ay,az
//增加互补滤波
ax=ax*kfa+kfan*axi;
ay=ay*kfa+kfan*ayi;
az=az*kfa+kfan*azi;
gx=gx*kfg+kfgn*gxi;
gy=gy*kfg+kfgn*gyi;
gz=gz*kfg+kfgn*gzi;
// 测量正常化
norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax / norm;
ay = ay / norm;
az = az / norm;
// 估计方向的重力
vx = 2*(q1*q3 - q0*q2);
vy = 2*(q0*q1 + q2*q3);
vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
// 错误的领域和方向传感器测量参考方向之间的交叉乘积的总和
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;
// 调整后的陀螺仪测量
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;
// 整合四元数率和正常化
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;
// 正常化四元
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
}
void IMU_quator_to_eula(float *ang_x,float *ang_y,float *ang_z)
{
*ang_x = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 );
*ang_y = -asinf( 2.0f * (q1 * q3 - q0 * q2) );
*ang_z = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 );
}
Q_ANGLE.Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
Q_ANGLE.Rool = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // rollv
//====================================================================================================
// END OF CODE
//====================================================================================================
这个程序已经被用烂了，的确是相当好用 |
|
发表于 2013-4-10 20:37:44
