資源簡介
MPU6050內(nèi)部運動引擎(DMP)資料 直接輸出四元數(shù) 可直接計算角度 數(shù)據(jù)無需進行卡爾曼濾波

代碼片段和文件信息
/*?Windows?API?串口函數(shù)與結(jié)構(gòu)體
CreateFile() //打開串口(打開成功返回串口資源句柄,失敗返回-1)
SetupComm() //設(shè)置串口讀寫的緩沖區(qū)大小的()
PurgeComm() //清空串口緩沖區(qū)
GetCommState() //獲取串口的初始配置
SetCommState() //配制串口各參數(shù)
GetCommTimeouts() //獲取串口超時當(dāng)前的配制
SetCommTimeouts() //配制串口超時函數(shù)(寫入COMMTIMEOUTS結(jié)構(gòu)體的數(shù)據(jù))
ReadFile() //讀串口數(shù)據(jù)
COMMTIMEOUTS //這個結(jié)構(gòu)體是設(shè)置關(guān)于串口超時方面的(寫入后才生效)
DCB //這個結(jié)構(gòu)提是設(shè)置關(guān)于串口各參數(shù)方面的(寫入后才生效)
*/
/*名詞術(shù)語表示
Quaternion?=?四元數(shù)
Yaw=偏航
Pitch=俯仰
Roll=翻滾
*/
#include?
#include?
#include?
int?dmpGetQuaternion1(int?*data?unsigned?char?*packet); //取出DMP四元數(shù)原始數(shù)據(jù)
int?dmpGetQuaternion2(float?*qunsigned?char*?packet);?//計算出四元數(shù)
void?dmpGetyawpitchroll(float?*yprfloat?*q); //計算歐拉角?RollPitchYaw
int?main(int?argc?char?*agrv[]) //字符最后一位為\0,
{
int?btl;
char?comk[6]=“com1“;
if(argc?>?1?)
{
btl?=?0;
while((agrv[1][btl]!=‘\0‘)?&&?(btl<5))
{
comk[btl]?=?agrv[1][btl];
btl++;
}
if(argc?>?2)
{
sscanf(agrv[2]“%d“&btl);
}
else
{
btl?=?9600;
}
}
else
{
btl?=?9600;
}
HANDLE?hCom; //保存串口句柄
hCom?=?CreateFile(comk //打開的串口名
GENERIC_READ|GENERIC_WRITE?//允許讀和寫(單讀或單寫也可以)
0?//獨占方式(必須的)
NULL //引用安全性屬性結(jié)構(gòu),缺省值為NULL
OPEN_EXISTING?//打開而不是創(chuàng)建
0?//同步方式
NULL); //對串口而言該參數(shù)必須置為NULL
if?(hCom==(HANDLE)-1)
{
printf(“打開串口失敗“);
return?-1;
}
SetupComm(hCom10241024);?//輸入緩沖區(qū)和輸出緩沖區(qū)的大小都是1024
COMMTIMEOUTS?TimeOuts;??//這里的只是設(shè)定,如果不寫入設(shè)定那么這個設(shè)定就不回生效
//設(shè)定讀超時??
TimeOuts.ReadIntervalTimeout=1000;??
TimeOuts.ReadTotalTimeoutMultiplier=500;??
TimeOuts.ReadTotalTimeoutConstant=5000;??
//設(shè)定寫超時??
TimeOuts.WriteTotalTimeoutMultiplier=500;??
TimeOuts.WriteTotalTimeoutConstant=2000;??
SetCommTimeouts(hCom&TimeOuts);?//設(shè)置超時??
DCB?dcb;
GetCommState(hCom&dcb); //獲取串口的初始配置
dcb.BaudRate=btl;?//波特率為9600??
dcb.ByteSize=8;?//每個字節(jié)有8位??
dcb.Parity=NOPARITY;?//無奇偶校驗位??
dcb.StopBits=1;?//兩個停止位??
SetCommState(hCom&dcb); //配制串口各參數(shù)
PurgeComm(hComPURGE_TXCLEAR|PURGE_RXCLEAR);?//清空串口緩沖區(qū)(讀與寫的一起清空)
//讀串口
unsigned?char?data[8];//
DWORD?wCount;//讀取的字節(jié)數(shù)?(成功的字符數(shù))
BOOL?bReadStat;??//
bReadStat=ReadFile(hComdata8&wCountNULL); //讀取字符
float?q[4]ypr[3];
while(1) //死循環(huán)輸出最新歐拉角
{
bReadStat=ReadFile(hComdata8&wCountNULL); //讀取字符
dmpGetQuaternion2(qdata); //取出四元數(shù)?W?X?Y?Z
dmpGetyawpitchroll(yprq);?//計算角度
//輸出?角度
printf(“roll=%f?“ypr[0]);
printf(“pitch=%f?“ypr[1]);
printf(“yaw=%f?\n“ypr[2]);
}
return?0;
}
void?dmpGetyawpitchroll(float?*yprfloat?*q) //計算角度
{
??//?Roll?=?Atan2(2?*(Y?*?Z?+?W?*?X)??W?*?W?-X?*?X?-Y?*?Y?+?Z?*?Z)?
?ypr[0]?=?atan2(2?*(q[2]?*?q[3]?+?q[0]?*?q[1])??q[0]?*?q[0]?-q[1]?*?q[1]?-q[2]?*?q[2]?+?q[3]?*?q[3])*57.3;?
??//?Pitch?=?asin(-2?*?(X?*?Z?-?W?*?Y))??
?ypr[1]=asin(-2*(q[1]*q[3]-q[0]*q[2]))*57.3;
??//?Yaw???=?atan2(2?*?(X?*?Y?+?W?*?Z)?W?*?W?+?X?*?X?-?Y?*?Y?-?Z?*?Z)?
?ypr[2]=atan2(2*(q[1]?*?q[2]?+?q[0]?*?q[3])?
?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????文件?????464849??2014-08-08?22:10??DMP總結(jié)資料\DMP加載步驟.gif
?????文件????1012962??2014-08-08?17:38??DMP總結(jié)資料\DMP設(shè)置數(shù)據(jù)寫入.bmp
?????文件????1333167??2014-05-22?00:23??DMP總結(jié)資料\mpu?6050?資料\MPU-60X0寄存器中文版V4.0.pdf
?????文件?????186803??2012-03-28?11:55??DMP總結(jié)資料\mpu?6050?資料\MPU6050-V1-SCH.jpg
?????文件??????58985??2012-03-28?11:54??DMP總結(jié)資料\mpu?6050?資料\MPU6050-V1.jpg
?????文件??????32768??2014-08-09?13:08??DMP總結(jié)資料\mpu?6050?資料\本代碼使用的MPU6050.JPG
?????文件??????30366??2014-08-09?12:14??DMP總結(jié)資料\mpu6050DMP.c
?????文件?????782990??2014-08-08?18:29??DMP總結(jié)資料\更新DMP.bmp
?????文件?????????85??2014-08-09?12:25??DMP總結(jié)資料\說明.txt
?????目錄??????????0??2014-08-09?13:08??DMP總結(jié)資料\mpu?6050?資料
?????目錄??????????0??2014-08-09?13:06??DMP總結(jié)資料
?????文件???????4439??2014-08-10?17:21??DMP總結(jié)資料\ck.cpp
-----------?---------??----------?-----??----
??????????????3907414????????????????????12
評論
共有 條評論