STM32串口+I2C读取光流传感器PX4FLOW数据 (amobbs.com 阿莫电子技术论坛) -

tim4146 发表于 2016-3-25 17:52:53

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

本帖最后由 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协议的问题，这个我没太折腾直接在本论坛找到了一个程序，感谢原作者
最核心的代码如下
void FLOW_MAVLINK(unsigned char data)
{

switch(s_flow)//这里是读出串口输出的连续26个字节数据
      {
case 0: if(data==0xFE)
                     s_flow=1;
                     break;
            case 1: if(data==0x1A)
                        { s_flow=2;}
                     else
                     s_flow=0;
                     break;
            case 2:
                     if(data_cnt<4)
{s_flow=2; FLOW_STATE=data;}
else
{data_cnt=0;s_flow=3;flow_buf=data;}
break;

            case 3:
if(FLOW_STATE==100)
{
if(data_cnt<26)
{
s_flow=3;
flow_buf=data;
}

else
{
data_cnt=0;
s_flow=4;
}
}
else
{
data_cnt=0;
s_flow=0;
}
                     break;
            case 4:get_one_fame=1;s_flow=0;data_cnt=0;break;
            default:s_flow=0;data_cnt=0;break;
      }//--end of s_uart


      if(get_one_fame)
      {
get_one_fame=0;
//实测flow_buf是最低位 flow_buf是最高位
//实际使用-就足以完成实验
   flow.time_sec=(flow_buf<<24)|(flow_buf<<16)|(flow_buf<<8)|(flow_buf);
         floattobyte=flow_buf;
            floattobyte=flow_buf;
            floattobyte=flow_buf;
            floattobyte=flow_buf;
            flow.flow_comp_x =ByteToFloat(floattobyte);

            floattobyte=flow_buf;
            floattobyte=flow_buf;
            floattobyte=flow_buf;
            floattobyte=flow_buf;
            flow.flow_comp_y =ByteToFloat(floattobyte);

            floattobyte=flow_buf;
            floattobyte=flow_buf;
            floattobyte=flow_buf;
            floattobyte=flow_buf;
flow.hight=ByteToFloat(floattobyte);//ground_distance    float    Ground distance in m. Positive value: distance known. Negative value: Unknown distance

flow.flow_x=(int16_t)((flow_buf)|(flow_buf<<8));
flow.flow_y=(int16_t)((flow_buf)|(flow_buf<<8));

flow.id=flow_buf;
      flow.quality=flow_buf; //Optical flow quality / confidence. 0: bad, 255: maximum quality

            }
}
为了得到是实时的速度，我只需要时间 x速度 y速度，因此最后用串口1输出的时候是这样的：
printf("t:%u",flow.time_sec/1000);//输出时间单位是ms
printf(" ");
printf("x:%f",flow.flow_comp_x*1000);//输出x方向速度单位mm/s
printf(" ");
   printf("y:%f\r\n",flow.flow_comp_y*1000);//输出y方向速度单位mm/s
这里要注意，固件里面时间用了一个u64数据类型，实测flow_buf是最低位 flow_buf是最高位，实际使用-就足以完成实验，毕竟做个实验几十秒就差不多了，最后根据需要的精度我,flow.time_sec/1000得到毫秒为单位的时间
经过小修改的工程代码在这里：

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

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

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

IIC_Wait_Ack();
IIC_Send_Byte(ReadAddr); //发送低地址
IIC_Wait_Ack();
IIC_Start();
IIC_Send_Byte(0X85);       //进入接收模式
IIC_Wait_Ack();
temp=IIC_Read_Byte(0);
IIC_Stop();//产生一个停止条件
return temp;
}
同理，我还是只需要时间、x速度、y速度，所以最好输出是这样：
PX4FLOW_Read(0,datatemp,22);//连续读出22字节
temp=datatemp+datatemp*256;
printf("flow_comp_m_x=%d",temp);//输出X速度
printf(" ");
temp=datatemp+datatemp*256;
printf("flow_comp_m_y=%d",temp);//输出Y速度
printf(" ");
printf("t=%d\r\n",datatemp);//输出时间

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

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

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

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

ghdnui 发表于 2016-4-11 11:32:21

感谢楼主分享

CXSYS_C102 发表于 2016-5-3 18:42:02

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

am391144374 发表于 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 );

jamesdeep 发表于 2016-10-3 16:23:38

顶一个

SWUST-NJC 发表于 2017-9-18 09:56:57

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

angtiepubu 发表于 2018-1-6 23:41:32

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

fcmer2016 发表于 2018-1-7 09:20:03

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

页: [1]

amobbs.com 阿莫电子技术论坛's Archiver

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