amoBBS 阿莫电子论坛

 找回密码
 注册
搜索
bottom↓
查看: 2767|回复: 5

STM32串口+I2C读取光流传感器PX4FLOW数据

[复制链接]
发表于 2016-3-25 17:52:53 | 显示全部楼层 |阅读模式
本帖最后由 tim4146 于 2016-3-25 17:54 编辑

为了检测地面移动设备的移动速度,我尝试使用了光流传感器,下面把我使用过程中的一些过程分享出来,如果可以的话麻烦给条裤子。

随便说几句
传感器是X宝买的,目测卖家只是个卖硬件的,固件部分卖家应该有过优化。图片是下面的,网址就不给了,以免说我做广告

传感器官方是有个配套地面站软件的,叫做QGroundControl 下载地址是:http://www.mavlink.org/downloads
这个传感器不能再64位的WIN7使用,我试了好几台都不行,试了第一台win7 32位就脸上了
传感器和QGroundControl的连接主要是验证传感器性能正常,并且通过图像来调节摄像头的焦距(如果需要的话)
关于通讯
PX4FLOW有三种对外通讯方式,上面说的和QGroundControl 的通讯是通过USB口,还有就是串口和I2C ,这个在官方是有介绍的

STM32读取数据有两种办法:1、串口 2、I2C 下面我一个个说
stm32串口读取数据
难点主要在于Marvlink协议的问题,这个我没太折腾直接在本论坛找到了一个程序,感谢原作者
最核心的代码如下
  1. void FLOW_MAVLINK(unsigned char data)
  2. {
  3.        
  4.     switch(s_flow)//这里是读出串口输出的连续26个字节数据
  5.          {
  6.                                                                 case 0: if(data==0xFE)
  7.                         s_flow=1;
  8.                         break;
  9.                 case 1: if(data==0x1A)
  10.                            { s_flow=2;}
  11.                         else
  12.                         s_flow=0;
  13.                         break;
  14.                 case 2:
  15.                         if(data_cnt<4)
  16.                                                                                                         {s_flow=2; FLOW_STATE[data_cnt++]=data;}
  17.                                                                                                 else
  18.                                                                                                         {data_cnt=0;s_flow=3;flow_buf[data_cnt++]=data;}
  19.                                                                                                 break;

  20.                 case 3:
  21.                                                                                                 if(FLOW_STATE[3]==100)
  22.                                                                                                         {
  23.                                                                                                                                 if(data_cnt<26)
  24.                                                                                                                                 {
  25.                                                                                                                                         s_flow=3;
  26.                                                                                                                                         flow_buf[data_cnt++]=data;
  27.                                                                                                                                 }
  28.                                                                                                                        
  29.                                                                                                                                         else
  30.                                                                                                                                 {
  31.                                                                                                                                         data_cnt=0;
  32.                                                                                                                                         s_flow=4;       
  33.                                                                                                                                 }
  34.                                                                                                                         }
  35.                                                                                                 else
  36.                                                                                                         {
  37.                                                                                                                         data_cnt=0;
  38.                                                                                                                         s_flow=0;
  39.                                                                                                         }
  40.                          break;
  41.                 case 4:get_one_fame=1;s_flow=0;data_cnt=0;break;
  42.                 default:s_flow=0;data_cnt=0;break;
  43.          }//--end of s_uart
  44.                

  45.          if(get_one_fame)
  46.          {
  47.                                                                 get_one_fame=0;
  48.                                          //实测flow_buf[0]是最低位 flow_buf[7]是最高位
  49.                                          //实际使用[0]-[3]就足以完成实验
  50.                                           flow.time_sec=(flow_buf[3]<<24)|(flow_buf[2]<<16)|(flow_buf[1]<<8)|(flow_buf[0]);
  51.                                          floattobyte[0]=flow_buf[8];
  52.                  floattobyte[1]=flow_buf[9];
  53.                  floattobyte[2]=flow_buf[10];
  54.                  floattobyte[3]=flow_buf[11];
  55.                  flow.flow_comp_x =ByteToFloat(floattobyte);
  56.                                          
  57.                  floattobyte[0]=flow_buf[12];
  58.                  floattobyte[1]=flow_buf[13];
  59.                  floattobyte[2]=flow_buf[14];
  60.                  floattobyte[3]=flow_buf[15];
  61.                  flow.flow_comp_y =ByteToFloat(floattobyte);
  62.                                          
  63.                  floattobyte[0]=flow_buf[16];
  64.                  floattobyte[1]=flow_buf[17];
  65.                  floattobyte[2]=flow_buf[18];
  66.                  floattobyte[3]=flow_buf[19];
  67.                                                                  flow.hight=ByteToFloat(floattobyte);//ground_distance        float        Ground distance in m. Positive value: distance known. Negative value: Unknown distance     
  68.                                          
  69.                                                                  flow.flow_x=(int16_t)((flow_buf[20])|(flow_buf[21]<<8));
  70.                                                                  flow.flow_y=(int16_t)((flow_buf[22])|(flow_buf[23]<<8));
  71.                                                                  

  72.                                
  73.                                                                  flow.id=flow_buf[24];
  74.                                                            flow.quality=flow_buf[25]; //Optical flow quality / confidence. 0: bad, 255: maximum quality
  75.         
  76.                 }
  77. }
复制代码

为了得到是实时的速度,我只需要时间 x速度 y速度,因此最后用串口1输出的时候是这样的:
  1.                                                                 printf("t:%u",flow.time_sec/1000);//输出时间  单位是ms
  2.                                                                         printf("    ");
  3.                                                                         printf("x:%f",flow.flow_comp_x*1000);//输出x方向速度 单位mm/s
  4.                                                                         printf("    ");
  5.                                                                   printf("y:%f\r\n",flow.flow_comp_y*1000);//输出y方向速度 单位mm/s
复制代码

这里要注意,固件里面时间用了一个u64数据类型,实测flow_buf[0]是最低位 flow_buf[7]是最高位,实际使用[0]-[3]就足以完成实验,毕竟做个实验几十秒就差不多了,最后根据需要的精度我,flow.time_sec/1000得到毫秒为单位的时间
经过小修改的工程代码在这里:


stm32的I2C读取数据
关于I2C的数据,我只关心速度部分,官方描述是这样的

那么对应的x方向速度就是地址0x06 0x07 Y方向速度地址就是 0x08 0x09低位在前 高位在后
关于I2C的设备地址是最容易出错的,官方这样说的

那么默认的地址就是0x42,发现不是,因为i2c是七位地址,所以需要左移一位才行,反映在代码就是下面这样:0x42左移一位就是0x84
  1. //在光流指定地址读出一个数据
  2. //ReadAddr:开始读数的地址  
  3. //返回值  :读到的数据
  4. u8 PX4FLOW_ReadOneByte(u16 ReadAddr)
  5. {                                  
  6.         u8 temp=0;                                                                                                                                                               
  7.     IIC_Start();  
  8. IIC_Send_Byte(0X84);   //发送器件地址0X42,写数据          

  9.         IIC_Wait_Ack();
  10.     IIC_Send_Byte(ReadAddr);   //发送低地址
  11.         IIC_Wait_Ack();            
  12.         IIC_Start();                     
  13.         IIC_Send_Byte(0X85);           //进入接收模式                          
  14.         IIC_Wait_Ack();         
  15.     temp=IIC_Read_Byte(0);                  
  16.     IIC_Stop();//产生一个停止条件            
  17.         return temp;
  18. }
复制代码

同理,我还是只需要时间、x速度、y速度,所以最好输出是这样:
  1.                   PX4FLOW_Read(0,datatemp,22);//连续读出22字节
  2.                 temp=datatemp[6]+datatemp[7]*256;
  3.                 printf("flow_comp_m_x=%d",temp);//输出X速度
  4.                 printf("    ");
  5.                 temp=datatemp[8]+datatemp[9]*256;
  6.                 printf("flow_comp_m_y=%d",temp);//输出Y速度
  7.                 printf("    ");
  8.                 printf("t=%d\r\n",datatemp[19]);//输出时间
复制代码

但是发现输出的时间貌似不是时间,官方的描述是“uint8_t sonar_timestamp;// time since last sonar update [milliseconds]”,单位是milliseconds 毫秒,但是类型是u8,想想应该也是不够时间记录的...
工程文件这样的:


实验数据简单介绍
因为I2C我暂时没法得到实时的系统时间,串口那边倒是可以,所以暂且用串口来测速的
光流传感器被我安装在一个四轮小车前端,地面是大理石,实验过程固定了LED灯照明,车体本身存在振动,最后的速度曲线是这样的

曲线可以看出速度有明显波动,其实小车没这么大波动,原因在于:
1、传感器本身存在测量误差,但是误差没这么大
2、振动导致数据波动。这个绝对是最大的因素,光流传感器及时在四轴使用也是很注意抗震的,在我的小车(Mecanum轮)上,车体本身的振动比四轴大很多,及时传感器内部集成加速度陀螺仪数据的补偿,还是会产生不小的影响吧。

转载请注明转自阿莫论坛
编辑原因:错别字改动

本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册

x
发表于 2016-4-11 11:32:21 | 显示全部楼层
感谢楼主分享
发表于 2016-5-3 18:42:02 | 显示全部楼层
我是新手,请问一下大家,I2C读取光流数据,通过串口打印 ,为什么数据不变化,偶尔变一下。。怎么回事儿啊?
发表于 2016-10-1 21:08:32 | 显示全部楼层
本帖最后由 am391144374 于 2016-10-1 21:17 编辑

楼主,试了你这个I2C读数据的程序是可以使用的。
但是用下面这种你在程序里注释掉的语句怎么就读不出数据了,是应为这样不行你才改掉的吗
// flow_comp_m_x_low=PX4FLOW_ReadOneByte(0x06);//flow_comp_m_x low
// flow_comp_m_x_high=PX4FLOW_ReadOneByte(0x07);//flow_comp_m_x high
//                 temp=flow_comp_m_x_low+flow_comp_m_x_high*256;
// printf("flow_comp_m_x=%d",temp);
// printf("    ");
// flow_comp_m_y_low=PX4FLOW_ReadOneByte(0x08);//flow_comp_m_x low
// flow_comp_m_y_high=PX4FLOW_ReadOneByte(0x09);//flow_comp_m_x high
// printf("flow_comp_m_y=%d\r\n",flow_comp_m_y_low+flow_comp_m_y_high*256 );
发表于 2016-10-3 16:23:38 | 显示全部楼层
顶一个     
发表于 2017-9-18 09:56:57 | 显示全部楼层
不错,楼主,我正在调试,给了我很大的帮助,谢谢
友情提示:标题不合格、重复发帖,将会被封锁ID。详情请参考:论坛通告:封锁ID、获得注册邀请码、恢复被封ID、投诉必读
您需要登录后才可以回帖 登录 | 注册

本版积分规则

手机版|Archiver|阿莫电子论坛(原ourAVR/ourDEV) ( 粤ICP备09047143号 )

GMT+8, 2017-10-24 14:06

阿莫电子论坛, 原"中国电子开发网"

© 2004-2016 www.amobbs.com, 原www.ourdev.cn, 原www.ouravr.com

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