飞行姿态计算

/******************** (C) COPYRIGHT 2014 ANO Tech *******************************
 * ×÷Õß  
#include "ANO_IMU.h"


ANO_IMU imu;


ANO_IMU::ANO_IMU()
{
}


//IMU³õʼ»¯
void ANO_IMU::Init()
{
//Â˲¨Æ÷²ÎÊý³õʼ»¯
filter_Init();
//´«¸ÐÆ÷³õʼ»¯
sensor_Init();
}


//¸üд«¸ÐÆ÷Êý¾Ý
void ANO_IMU::updateSensor()
{
//¶ÁÈ¡¼ÓËÙ¶È
mpu6050.Read_Acc_Data();
//¶ÁÈ¡½ÇËÙ¶È
mpu6050.Read_Gyro_Data();
//»ñÈ¡½ÇËٶȣ¬µ¥Î»Îª¶ÈÿÃë
Gyro = mpu6050.Get_Gyro_in_dps();
//»ñÈ¡¼ÓËٶȲÉÑùÖµ
Acc = mpu6050.Get_Acc();
}




//¼ÆËã·ÉÐÐÆ÷×Ë̬
void ANO_IMU::getAttitude()
{
float deltaT;

#ifdef ANO_IMU_USE_LPF_1st
//¼ÓËÙ¶ÈÊý¾ÝÒ»½×µÍͨÂ˲¨
Acc_lpf = LPF_1st(Acc_lpf, Acc, ano.factor.acc_lpf);
#endif

#ifdef ANO_IMU_USE_LPF_2nd
//¼ÓËÙ¶ÈÊý¾Ý¶þ½×µÍͨÂ˲¨
Acc_lpf = LPF_2nd(&Acc_lpf_2nd, Acc);
#endif

deltaT = getDeltaT(GetSysTime_us());

#ifdef ANO_IMU_USE_DCM_CF
DCM_CF(Gyro,Acc_lpf,deltaT);
#endif
#ifdef ANO_IMU_USE_Quaternions_CF
Quaternion_CF(Gyro,Acc_lpf_1st,deltaT);
#endif
}




//ÓàÏÒ¾ØÕó¸üÐÂ×Ë̬
void ANO_IMU::DCM_CF(Vector3f gyro,Vector3f acc, float deltaT)
{
static Vector3f deltaGyroAngle, LastGyro;
static Vector3f Vector_G(0, 0, ACC_1G), Vector_M(1000, 0, 0);
Matrix3f dcm;

//¼ÆËãÍÓÂÝÒǽǶȱ仯£¬¶þ½×Áú¸ñ¿âËþ»ý·Ö
deltaGyroAngle = (gyro + LastGyro) * 0.5 * deltaT;
LastGyro = gyro;

//¼ÆËã±íʾµ¥´ÎÐýתµÄÓàÏÒ¾ØÕó
dcm.from_euler(deltaGyroAngle);

//ÀûÓÃÓàÏÒ¾ØÕó¸üÐÂÖØÁ¦ÏòÁ¿ÔÚ»úÌå×ø±êϵµÄͶӰ
Vector_G = dcm * Vector_G;

//ÀûÓÃÓàÏÒ¾ØÕó¸üеشÅÏòÁ¿ÔÚ»úÌå×ø±êϵµÄͶӰ
Vector_M = dcm * Vector_M;

//»¥²¹Â˲¨£¬Ê¹ÓüÓËٶȲâÁ¿Öµ½ÃÕý½ÇËٶȻý·ÖƯÒÆ
Vector_G = CF_1st(Vector_G, acc, ano.factor.gyro_cf);


//¼ÆËã·ÉÐÐÆ÷µÄROLLºÍPITCH
Vector_G.get_rollpitch(angle);

//¼ÆËã·ÉÐÐÆ÷µÄYAW
Vector_M.get_yaw(angle);
}




#define Kp 2.0f        //¼ÓËÙ¶ÈȨÖØ£¬Ô½´óÔòÏò¼ÓËٶȲâÁ¿ÖµÊÕÁ²Ô½¿ì
#define Ki 0.001f      //Îó²î»ý·ÖÔöÒæ
//ËÄÔªÊý¸üÐÂ×Ë̬
void ANO_IMU::Quaternion_CF(Vector3f gyro,Vector3f acc, float deltaT)
{
Vector3f V_gravity, V_error, V_error_I;

//ÖØÁ¦¼ÓËٶȹéÒ»»¯
acc.normalize();

//ÌáÈ¡ËÄÔªÊýµÄµÈЧÓàÏÒ¾ØÕóÖеÄÖØÁ¦·ÖÁ¿
Q.vector_gravity(V_gravity);

//ÏòÁ¿²æ»ýµÃ³ö×Ë̬Îó²î
V_error = acc % V_gravity;

//¶ÔÎó²î½øÐлý·Ö
V_error_I += V_error * Ki;

//»¥²¹Â˲¨£¬×Ë̬Îó²î²¹³¥µ½½ÇËÙ¶ÈÉÏ£¬ÐÞÕý½ÇËٶȻý·ÖƯÒÆ
Gyro += V_error * Kp + V_error_I;

//Ò»½×Áú¸ñ¿âËþ·¨¸üÐÂËÄÔªÊý
Q.Runge_Kutta_1st(Gyro, deltaT);

//ËÄÔªÊý¹éÒ»»¯
Q.normalize();

//ËÄÔªÊýתŷÀ­½Ç
Q.to_euler(&angle.x, &angle.y, &angle.z);
}


void ANO_IMU::filter_Init()
{
//¼ÓËÙ¶ÈÒ»½×µÍͨÂ˲¨Æ÷ϵÊý¼ÆËã
ano.factor.acc_lpf = LPF_1st_Factor_Cal(IMU_LOOP_TIME * 1e-6, ACC_LPF_CUT);

//¼ÓËٶȶþ½×µÍͨÂ˲¨Æ÷ϵÊý¼ÆËã
LPF_2nd_Factor_Cal(&Acc_lpf_2nd);

//»¥²¹Â˲¨Æ÷ϵÊý¼ÆËã
ano.factor.gyro_cf = CF_Factor_Cal(IMU_LOOP_TIME * 1e-6, GYRO_CF_TAU);
}


void ANO_IMU::sensor_Init()
{
//³õʼ»¯MPU6050£¬1Khz²ÉÑùÂÊ£¬42HzµÍͨÂ˲¨
mpu6050.Init(1000,42);
}


float ANO_IMU::getDeltaT(uint32_t currentT)
{
static uint32_t previousT;
float deltaT = (currentT - previousT) * 1e-6;
previousT = currentT;

return deltaT;
}


/******************* (C) COPYRIGHT 2014 ANO TECH *****END OF FILE************/