参考h e u y c k与dongfang的姿态算法，我也弄了一个姿态解算出来玩。。
///////////////////////////////////////
#define PI 3.141592653589793
#define f 0.0216 //采样速度
#define rad 57.29578//180/PI
int cons;
////////////////////////////////////
extern float gx, gy, gz;
extern float ax, ay, az;
///////////////////////////////////////
float w, x, y, z;
float b_w, b_x, b_y, b_z;
float c_w, c_x, c_y, c_z;
float ea_fRoll, ea_fPitch, ea_fYaw;
float Roll, Pitch, Yaw;
////////////////////////////////////////////////////
/////////////////////////////////////////////////////////
float ea_EulerAngle[3];
float new_EulerAngle[3];
float ea_Quaternion[4];
float new_Quaternion[4];
/////////////////////////////////////////////////////
//----------------------------------------------------------------------------------------------------
// Definitions
#define Kp 4.0f // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 0.005f // integral gain governs rate of convergence of gyroscope biases
#define halfT 0.01f // half the sample period
//---------------------------------------------------------------------------------------------------
// Variable definitions
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
//====================================================================================================
// Function
//====================================================================================================
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) {
float norm;
float vx, vy, vz;
float ex, ey, ez;
// normalise the measurements
norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax / norm;
ay = ay / norm;
az = az / norm;
// estimated direction of gravity
vx = 2*(q1*q3 - q0*q2);
vy = 2*(q0*q1 + q2*q3);
vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
// error is sum of cross product between reference direction of field and direction measured by sensor
ex = (ay*vz - az*vy);
ey = (az*vx - ax*vz);
ez = (ax*vy - ay*vx);
// integral error scaled integral gain
exInt = exInt + ex*Ki;
eyInt = eyInt + ey*Ki;
ezInt = ezInt + ez*Ki;
// adjusted gyroscope measurements
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;
// 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;
q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
// normalise quaternion
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
}
//////////////////////////////////////////////////////////////////////////
void Quaternion_FromEulerAngle( float ea_fRoll,float ea_fPitch,float ea_fYaw) //欧拉转四元
{
float fCosHRoll = cos(ea_fRoll * 0.5f);
float fSinHRoll = sin(ea_fRoll * 0.5f);
float fCosHPitch = cos(ea_fPitch * 0.5f);
float fSinHPitch = sin(ea_fPitch * 0.5f);
float fCosHYaw = cos(ea_fYaw * 0.5f);
float fSinHYaw = sin(ea_fYaw * 0.5f);
w = fCosHRoll * fCosHPitch * fCosHYaw + fSinHRoll * fSinHPitch * fSinHYaw;
x = fCosHRoll * fSinHPitch * fCosHYaw + fSinHRoll * fCosHPitch * fSinHYaw;
y = fCosHRoll * fCosHPitch * fSinHYaw - fSinHRoll * fSinHPitch * fCosHYaw;
z = fSinHRoll * fCosHPitch * fCosHYaw - fCosHRoll * fSinHPitch * fSinHYaw;
}
Quaternion_ToEulerAngle(float w,float x,float y,float z) //四元转欧拉
{
ea_fRoll = atan2(2 * (w * z + x * y) , 1 - 2 * (z * z + x * x));
ea_fPitch = asin(2 * (w * x - y * z));
ea_fYaw = atan2(2 * (w * y + z * x) , 1 - 2 * (x * x + y * y));
}
main()
{
Stm32_Clock_Init(9); //系统时钟设置
delay_init(72); //延时初始化
LED_Init(); //初始化与LED连接的硬件接口
Adc_Init();
uart_init(72,115200);
//Timerx_Init(200,7199);//计数到2048为2048us 步进 1us
//EXTIX_Init();
JTAG_Set(SWD_ENABLE); //关闭JTAG,打开swd 释放出三个JTAG的口,做普通IO口线
read_inti_gyro();
ACCRead(); //读取acc
ea_EulerAngle[0]=atan2(ax,az); //根据acc算出欧拉（弧度）
// ea_EulerAngle[1]= asin(ay / 0.981);
// ea_EulerAngle[0]=-atan(ax/sqrt((ay*ay)+(az*az)));
ea_EulerAngle[1]=atan(ay/sqrt((ax*ax)+(az*az)));
ea_EulerAngle[2]=0;
Quaternion_FromEulerAngle( ea_EulerAngle[0],ea_EulerAngle[1],ea_EulerAngle[2] ); //欧拉转四元 初始值
q0=w;
q1=x;
q2=y; //放入初始化数组
q3=z;
// Quaternion_ToEulerAngle(q0,q1,q2,q3); //四元转欧拉(弧度)
// Roll=ea_fRoll*rad; //弧度转化为度 便于测试观察
// Pitch=ea_fPitch*rad;
// Yaw=ea_fYaw*rad;
delay_ms(20);
while(1)
{test=!test;
GyroRead(); //读取gyro
ACCRead(); //读取acc
new_EulerAngle[0]=(gx/rad*f); //读取到的为度/s /pi/180
new_EulerAngle[1]=(gy/rad*f);
new_EulerAngle[2]=(gz/rad*f);
IMUupdate(new_EulerAngle[0],new_EulerAngle[1],new_EulerAngle[2],ax,ay,az); //根据刷新gyro和acc计算新的四元姿态
Quaternion_ToEulerAngle(q0,q1,q2,q3); //四元转欧拉(弧度)
Yaw=ea_fRoll*rad; //弧度转化为度 xyz传感器方向不同调节了x与z的位置
Pitch=ea_fPitch*rad;
Roll=ea_fYaw*rad;
LED1=!LED1;
delay_us(19500);
if(Roll>30)LED0=0; //ROLL大于30度时指示
else LED0=1;
}
}
感觉基本正常，有待进一步测试。。。
[ 本帖最后由 峰回路转 于 2012-3-12 13:26 编辑 ] |
/2 
发表于 2012-3-12 13:25
收藏
楼主
