对卡尔曼滤波的串口输出mpu6050示例程序的修改
买了个卡尔曼滤波的串口输出的mpu6050，看说明是每10ns输出一次数据，一次输出33个字节，分三个包按加速度，角速度，角度顺序输出。但是示例程序是每次接收11个字节，虽然这样不影响数据的提取，但是造成每次程序运行时间不一致。修改后，10ns更新一次数据，程序也10ns运行一次。
unsigned char Re_buf,counter=0;
unsigned long starttime, stoptime;
int looptime;
unsigned char sign=0;
float a,w,angle,T;
void setup() {
// initialize serial:
Serial.begin(115200);
}
void loop() {
starttime = millis();
if(sign)
{
sign=0;
if(Re_buf==0x55) //检查帧头
{
本帖最后由 tuteng8601 于 2020-2-20 21:06 编辑
a = (short(Re_buf <<8| Re_buf ))/32768.0*16;
a = (short(Re_buf <<8| Re_buf ))/32768.0*16;
a = (short(Re_buf <<8| Re_buf ))/32768.0*16;
T = (short(Re_buf <<8| Re_buf ))/340.0+36.25;
w = (short(Re_buf <<8| Re_buf ))/32768.0*2000;
w = (short(Re_buf <<8| Re_buf ))/32768.0*2000;
w = (short(Re_buf <<8| Re_buf ))/32768.0*2000;
T = (short(Re_buf <<8| Re_buf ))/340.0+36.25;
angle = (short(Re_buf <<8| Re_buf ))/32768.0*180;
angle = (short(Re_buf <<8| Re_buf ))/32768.0*180;
angle = (short(Re_buf <<8| Re_buf ))/32768.0*180;
T = (short(Re_buf <<8| Re_buf ))/340.0+36.25;
Serial.print("a:");
Serial.print(a);Serial.print(" ");
Serial.print(a);Serial.print(" ");
Serial.print(a);Serial.print(" ");
Serial.print("w:");
Serial.print(w);Serial.print(" ");
Serial.print(w);Serial.print(" ");
Serial.print(w);Serial.print(" ");
Serial.print("angle:");
Serial.print(angle);Serial.print(" ");
Serial.print(angle);Serial.print(" ");
Serial.print(angle);Serial.print(" ");
Serial.print("T:");
Serial.println(T);
}
}
stoptime = millis();
looptime =int( stoptime - starttime);
Serial.print('m');
Serial.println(looptime);
}
void serialEvent() {
while (Serial.available()) {
//char inChar = (char)Serial.read(); Serial.print(inChar); //Output Original Data, use this code
Re_buf=(unsigned char)Serial.read();
if(counter==0&&Re_buf!=0x55) return; //第0号数据不是帧头
if(counter==1&&Re_buf!=0x51) return; //检查是不是加速度包
counter++;
if(counter==33) //接收到33个数据
{
counter=0; //重新赋值，准备下一帧数据的接收
sign=1;
}
}
} 学习了，感谢分享。
页:
[1]