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

tim4146

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

随便说几句
传感器是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. 
    
21.                 case 3:
    
22.                                                                                                 if(FLOW_STATE[3]==100)
    
23.                                                                                                         {
    
24.                                                                                                                                 if(data_cnt<26)
    
25.                                                                                                                                 {
    
26.                                                                                                                                         s_flow=3;
    
27.                                                                                                                                         flow_buf[data_cnt++]=data;
    
28.                                                                                                                                 }
    
29.                                                                                                                        
    
30.                                                                                                                                         else
    
31.                                                                                                                                 {
    
32.                                                                                                                                         data_cnt=0;
    
33.                                                                                                                                         s_flow=4;       
    
34.                                                                                                                                 }
    
35.                                                                                                                         }
    
36.                                                                                                 else
    
37.                                                                                                         {
    
38.                                                                                                                         data_cnt=0;
    
39.                                                                                                                         s_flow=0;
    
40.                                                                                                         }
    
41.                          break;
    
42.                 case 4:get_one_fame=1;s_flow=0;data_cnt=0;break;
    
43.                 default:s_flow=0;data_cnt=0;break;
    
44.          }//--end of s_uart
    
45.                
    
46. 
    
47.          if(get_one_fame)
    
48.          {
    
49.                                                                 get_one_fame=0;
    
50.                                          //实测flow_buf[0]是最低位 flow_buf[7]是最高位
    
51.                                          //实际使用[0]-[3]就足以完成实验
    
52.                                           flow.time_sec=(flow_buf[3]<<24)|(flow_buf[2]<<16)|(flow_buf[1]<<8)|(flow_buf[0]);
    
53.                                          floattobyte[0]=flow_buf[8];
    
54.                  floattobyte[1]=flow_buf[9];
    
55.                  floattobyte[2]=flow_buf[10];
    
56.                  floattobyte[3]=flow_buf[11];
    
57.                  flow.flow_comp_x =ByteToFloat(floattobyte);
    
58.                                          
    
59.                  floattobyte[0]=flow_buf[12];
    
60.                  floattobyte[1]=flow_buf[13];
    
61.                  floattobyte[2]=flow_buf[14];
    
62.                  floattobyte[3]=flow_buf[15];
    
63.                  flow.flow_comp_y =ByteToFloat(floattobyte);
    
64.                                          
    
65.                  floattobyte[0]=flow_buf[16];
    
66.                  floattobyte[1]=flow_buf[17];
    
67.                  floattobyte[2]=flow_buf[18];
    
68.                  floattobyte[3]=flow_buf[19];
    
69.                                                                  flow.hight=ByteToFloat(floattobyte);//ground_distance        float        Ground distance in m. Positive value: distance known. Negative value: Unknown distance     
    
70.                                          
    
71.                                                                  flow.flow_x=(int16_t)((flow_buf[20])|(flow_buf[21]<<8));
    
72.                                                                  flow.flow_y=(int16_t)((flow_buf[22])|(flow_buf[23]<<8));
    
73.                                                                  
    
74. 
    
75.                                
    
76.                                                                  flow.id=flow_buf[24];
    
77.                                                            flow.quality=flow_buf[25]; //Optical flow quality / confidence. 0: bad, 255: maximum quality
    
78.         
    
79.                 }
    
80. }

复制代码

为了得到是实时的速度，我只需要时间 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. 
   
10.         IIC_Wait_Ack();
    
11.     IIC_Send_Byte(ReadAddr);   //发送低地址
    
12.         IIC_Wait_Ack();            
    
13.         IIC_Start();                     
    
14.         IIC_Send_Byte(0X85);           //进入接收模式                          
    
15.         IIC_Wait_Ack();         
    
16.     temp=IIC_Read_Byte(0);                  
    
17.     IIC_Stop();//产生一个停止条件            
    
18.         return temp;
    
19. }

复制代码

同理，我还是只需要时间、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轮）上，车体本身的振动比四轴大很多，及时传感器内部集成加速度陀螺仪数据的补偿，还是会产生不小的影响吧。

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

ghdnui

感谢楼主分享

CXSYS_C102

我是新手，请问一下大家，I2C读取光流数据，通过串口打印 ，为什么数据不变化，偶尔变一下。。怎么回事儿啊？

am391144374

楼主，试了你这个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 );

jamesdeep

顶一个     

SWUST-NJC

不错，楼主，我正在调试，给了我很大的帮助，谢谢

angtiepubu

请问楼主知道怎么通过串口设置相机参数吗

fcmer2016

现在光流越来用的越多了。