极客工坊

 找回密码
 注册

QQ登录

只需一步,快速开始

查看: 30577|回复: 2

对卡尔曼滤波的串口输出mpu6050示例程序的修改

[复制链接]
发表于 2020-2-20 20:09:47 | 显示全部楼层 |阅读模式
买了个卡尔曼滤波的串口输出的mpu6050,看说明是每10ns输出一次数据,一次输出33个字节,分三个包按加速度,角速度,角度顺序输出。但是示例程序是每次接收11个字节,虽然这样不影响数据的提取,但是造成每次程序运行时间不一致。
修改后,10ns更新一次数据,程序也10ns运行一次。
unsigned char Re_buf[33],counter=0;
unsigned long starttime, stoptime;
int looptime;
unsigned char sign=0;
float a[3],w[3],angle[3],T;
void setup() {
  // initialize serial:
  Serial.begin(115200);
}

void loop() {
  starttime = millis();
  if(sign)

  {   
     sign=0;
     if(Re_buf[0]==0x55)      //检查帧头
     {  
回复

使用道具 举报

 楼主| 发表于 2020-2-20 20:11:13 | 显示全部楼层
本帖最后由 tuteng8601 于 2020-2-20 21:06 编辑

                a[0] = (short(Re_buf [3]<<8| Re_buf [2]))/32768.0*16;
                a[1] = (short(Re_buf [5]<<8| Re_buf [4]))/32768.0*16;
                a[2] = (short(Re_buf [7]<<8| Re_buf [6]))/32768.0*16;
                T = (short(Re_buf [9]<<8| Re_buf [8]))/340.0+36.25;
       
                w[0] = (short(Re_buf [14]<<8| Re_buf [13]))/32768.0*2000;
                w[1] = (short(Re_buf [16]<<8| Re_buf [15]))/32768.0*2000;
                w[2] = (short(Re_buf [18]<<8| Re_buf [17]))/32768.0*2000;
                T = (short(Re_buf [20]<<8| Re_buf [19]))/340.0+36.25;
       
                angle[0] = (short(Re_buf [25]<<8| Re_buf [24]))/32768.0*180;
                angle[1] = (short(Re_buf [27]<<8| Re_buf [26]))/32768.0*180;
                angle[2] = (short(Re_buf [29]<<8| Re_buf [28]))/32768.0*180;
                T = (short(Re_buf [31]<<8| Re_buf [30]))/340.0+36.25;
                Serial.print("a:");
                Serial.print(a[0]);Serial.print(" ");
                Serial.print(a[1]);Serial.print(" ");
                Serial.print(a[2]);Serial.print(" ");
                Serial.print("w:");
                Serial.print(w[0]);Serial.print(" ");
                Serial.print(w[1]);Serial.print(" ");
                Serial.print(w[2]);Serial.print(" ");
                Serial.print("angle:");
                Serial.print(angle[0]);Serial.print(" ");
                Serial.print(angle[1]);Serial.print(" ");
                Serial.print(angle[2]);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[counter]=(unsigned char)Serial.read();
    if(counter==0&&Re_buf[0]!=0x55) return;      //第0号数据不是帧头  
    if(counter==1&&Re_buf[1]!=0x51) return;       //检查是不是加速度包      
    counter++;      
    if(counter==33)             //接收到33个数据
    {   
       counter=0;               //重新赋值,准备下一帧数据的接收
       sign=1;
    }
      
  }
  
}
回复 支持 反对

使用道具 举报

发表于 2021-1-22 19:56:22 | 显示全部楼层
学习了,感谢分享。
回复 支持 反对

使用道具 举报

您需要登录后才可以回帖 登录 | 注册

本版积分规则 需要先绑定手机号

Archiver|联系我们|极客工坊

GMT+8, 2024-3-29 14:33 , Processed in 0.037610 second(s), 19 queries .

Powered by Discuz! X3.4 Licensed

Copyright © 2001-2021, Tencent Cloud.

快速回复 返回顶部 返回列表