|
|
#include <SoftwareSerial.h>
SoftwareSerial mySerial(12,13);//第12= RX,第13- TX
int YPR[3];
unsigned char Re_buf[8],counter=0;
unsigned char sign=0;
//-----------------------------------------------------------
void setup()
{
Serial.begin(115200);
mySerial.begin(115200);
delay(2000);
mySerial.write(0XA5);
mySerial.write(0X52); //初始化GY25,連續(xù)輸出模式
}
//-------------------------------------------------------------
void loop() {
mySerial.listen();
if(sign)
{
sign=0;
if(Re_buf[0]==0xAA && Re_buf[7]==0x55) //檢查幀頭，幀尾
{
YPR[0]=(Re_buf[1]<<8|Re_buf[2])/100; //合成數(shù)據(jù)，去掉小數(shù)點(diǎn)后2位
YPR[1]=(Re_buf[3]<<8|Re_buf[4])/100;
YPR[2]=(Re_buf[5]<<8|Re_buf[6])/100;
mySerial.print("航向：");
mySerial.println(YPR[0]); //顯示航向
mySerial.print("俯仰角：");
mySerial.println(YPR[1]); //顯示航向
mySerial.print("翻滾角：");
mySerial.println(YPR[2]); //顯示航向
delay(100);
}
}
}
//---------------------------------------------------------------
void serialEvent() {
while (mySerial.available()) {
Re_buf[counter]=(unsigned char)mySerial.read();
if(counter==0&&Re_buf[0]!=0xAA) return; // 檢查幀頭
counter++;
if(counter==8) //接收到數(shù)據(jù)
{
counter=0; //重新賦值，準(zhǔn)備下一幀數(shù)據(jù)的接收
sign=1;
}
}
}
|
|
