优象激光光流二合一T2模块使用体验记录及飞控端数据解析历程

目录

一、使用体验

二、飞控端数据解析获取部分

1、光流数据字节含义分析

2、通过串口通信对光流原始数据获取、解析

①在串口中断中获取数据，将其存入串口环形缓冲区中，每次获取14个字节，此处用串口0;

 ②对获取数据的帧头帧尾进行校验，若检验通过则将数据取出作为原始数据

三、光流数据飞控端的处理

1.只使用光流纯数据的处理方法

2.光流与加速度计融合的处理方法

一、使用体验

光流模块在飞控系统中通常用于无人机或其他自主飞行设备。它的主要作用是通过视觉感知来测量飞行器相对于地面的运动，特别是在低高度和室内环境中。经过多方面考虑，我们选择了优象的激光光流二合一T2模块。

模块优点：

体积小巧，质量小，便于装上飞行器；

功能集成性强，结合了激光测距和光流两个功能，同时只占用一个串口，缓解了我们当时无人机串口紧张的情况；

定点能力、抗强光干扰能力强；

优象光流的用户使用手册非常优秀详细，使用过程中遇到的绝大部分问题都可以在手册中找到解答，其中也包括了飞控端调试光流方法说明，为我们对飞控的学习和对光流模块的理解提高了很大的帮助。

优象激光光流二合一模块淘宝店铺链接https://m.tb.cn/h.ghwqHFqOqmV6nb1?tk=wLOy30q6Hyg

二、飞控端数据解析获取部分

1、光流数据字节含义分析

2、通过串口通信对光流原始数据获取、解析

①在串口中断中获取数据，将其存入串口环形缓冲区中，每次获取14个字节，此处用串口0;

//相关变量定义
#define  RINGBUFF_LEN   200 
typedef struct
{
    uint16_t Head;
    uint16_t Tail;
    uint16_t Lenght;
    uint8_t  Ring_Buff[RINGBUFF_LEN];
} RingBuff_t;

RingBuff_t COM0_Rx_Buf,COM1_Rx_Buf,COM2_Rx_Buf,COM3_Rx_Buf;


//串口中断函数
void UART0_IRQHandler(void)
{
    uint32_t flag = UARTIntStatus(UART0_BASE,1);//获取中断标志 原始中断状态 屏蔽中断标志
    UARTIntClear(UART0_BASE,flag);//清除中断标志
    while(UARTCharsAvail(UART0_BASE))//判断FIFO是否还有数据
    {
        RingBuf_Write(UARTCharGet(UART0_BASE),&COM0_Rx_Buf,14);//往环形队列里面写数据，每次获取14个字节
    }
}

//写入函数，将数据写入串口环形缓冲区中

void RingBuf_Write(unsigned char data,RingBuff_t *ringBuff,uint16_t Length)
{
    ringBuff->Ring_Buff[ringBuff->Tail]=data;//从尾部追加
    if(++ringBuff->Tail >= Length)//尾节点偏移
        ringBuff->Tail=0;//大于数组最大长度 归零 形成环形队列
    if(ringBuff->Tail==ringBuff->Head)//如果尾部节点追到头部节点，则修改头节点偏移位置丢弃早期数据
    {
        if((++ringBuff->Head)  >= Length)
            ringBuff->Head=0;
    }
}

 ②对获取数据的帧头帧尾进行校验，若检验通过则将数据取出作为原始数据

//相关变量定义
#define OpticalFlow_Ringbuf  COM0_Rx_Buf

typedef struct {

    signed short pixel_flow_x_integral;//像素光流x方向的积分
    signed short pixel_flow_y_integral;//像素光流y方向的积分
	unsigned short ground_distance;    //对地距离
	unsigned short flow_trust;         //光流状态值
	unsigned short laser_valid;        //激光测距置信度
    unsigned int integration_timespan;//相邻两次测量的间隔时间
    unsigned char qual;

} flow_integral_frame;

flow_integral_frame opt_origin_data;//光流原始数据


//光流数据解析函数
uint8_t Optflow_Prase()
{
    //帧头帧尾校验，若通过校验则取出数据
    for(uint16_t i=0; i<14; i++)
    {
        if(OpticalFlow_Ringbuf.Ring_Buff[i]==0xfe//帧头
               &&OpticalFlow_Ringbuf.Ring_Buff[i+1]==0x0a//帧头
               &&OpticalFlow_Ringbuf.Ring_Buff[i+13]==0x55)//帧尾
        {
                //光流原始数据获取
             opt_origin_data.pixel_flow_x_integral=(int16_t)(OpticalFlow_Ringbuf.Ring_Buff[i+3]<<8)|OpticalFlow_Ringbuf.Ring_Buff[i+2];

             opt_origin_data.pixel_flow_y_integral=(int16_t)(OpticalFlow_Ringbuf.Ring_Buff[i+5]<<8)|OpticalFlow_Ringbuf.Ring_Buff[i+4];

             opt_origin_data.ground_distance=(int16_t)(OpticalFlow_Ringbuf.Ring_Buff[i+9]<<8)|OpticalFlow_Ringbuf.Ring_Buff[i+8];
			 opt_origin_data.laser_valid=OpticalFlow_Ringbuf.Ring_Buff[i+10];
		     opt_origin_data.flow_trust=OpticalFlow_Ringbuf.Ring_Buff[i+11];

        }
    }  
}

三、光流数据飞控端的处理

光流数据在飞控端的处理可以分为两种情况，一是只使用光流纯数据；二是

将光流数据与加速度计进行融合。

1.只使用光流纯数据的处理方法

直接使用纯光流数据进行控制，平滑性与抗环境干扰性会差一些，但好处是处理简单，需要调试的参数少

opt_data.x=(opt_origin_data.pixel_flow_x_integral*opticalflow_high)/10000.0f;//单位:乘以高度单位mm后为实际位移mm
opt_data.y=(opt_origin_data.pixel_flow_y_integral*opticalflow_high)/10000.0f;//单位:乘以高度单位mm后为实际位移mm

//时间间隔
opt_data.dt=(int16_t)(opt_origin_data.integration_timespan*0.001f);//单位ms

Optflow.div=(tfdata.distance-tfdata.last_distance)/opt_data.dt; //观测速度
Optflow.acc=(tfdata.div-tfdata.last_div)/opt_data.dt;			//观测加速度
Optflow.last_distance=tfdata.distance;					//观测距离保存
Optflow.last_div=tfdata.div;							//观测速度保存

2.光流与加速度计融合的处理方法

当单独使用光流数据时，通常能够实现良好的静态悬停效果。然而，若要达到更高级的功能，比如提升抗风性能、精确的位移控制以及目标跟随能力，就需要将光流数据与加速度计数据融合，以获取更为精确、平滑和实时的信息。

typedef struct 
{
    float distance;
    float last_distance;
    float div;
    float acc;
    float last_div;
}Optflow;

typedef struct 
{
    float x;
    float y;
    unsigned short dt;
    unsigned char qual;
    unsigned char update;
} flow_float;

typedef struct
{
    float x;
    float y;
}Vector2f;

typedef struct
{
    float a[3];
    float b[3];
} Butter_Parameter;

typedef struct
{
    float Input_Butter[3];
    float Output_Butter[3];
} Butter_BufferData;

Vector2f opt_filter_data; //滤波后的像素位移
Vector2f opt_gyro_data;   //光流角速度
Vector2f opt_gyro_filter_data;//光流经过旋转互补滤波后的角速度
Butter_Parameter OpticalFlow_Parameter,OpticalFlow_Gyro_Parameter;
Butter_BufferData Buffer_OpticalFlow[2],Buffer_OpticalFlow_Gyro[2];
flow_float opt_data;

float OpticalFlow_Rotate_Complementary_Filter(float optflow_gyro,float gyro,uint8_t axis)//光流旋转互补滤波器
{
    float optflow_gyro_filter=0;
    
    if(axis=='x') optflow_gyro_filter=optflow_gyro-constrain_float(gyro,-4.0f,4.0f);//4
    else optflow_gyro_filter=optflow_gyro-constrain_float(gyro,-4.0f,4.0f);

    return optflow_gyro_filter;
}

float constrain_float(float amt, float low, float high)
{
    if (isnan(amt)) 
    {
        return (low+high)*0.5f;
    }
    return ((amt)<(low)?(low):((amt)>(high)?(high):(amt)));
}

//以下部分写在Optflow_Prase()光流数据解析函数中

//加速度计低通滤波器数据
opt_filter_data.x=LPButterworth(opt_origin_data.pixel_flow_x_integral,&Buffer_OpticalFlow[0],&OpticalFlow_Parameter);
opt_filter_data.y=LPButterworth(opt_origin_data.pixel_flow_y_integral,&Buffer_OpticalFlow[1],&OpticalFlow_Parameter);

//实际位移
opt_data.qual=opt_origin_data.qual;
opt_gyro_data.x=(opt_filter_data.x)/200.0f;//光流角速度rad/s
opt_gyro_data.y=(opt_filter_data.y)/200.0f;//光流角速度rad/s

gyro_filter_data.x=LPButterworth(Roll_Gyro,&Buffer_OpticalFlow_Gyro[0],&OpticalFlow_Gyro_Parameter)/57.3f;//陀螺仪相位同步角速度
gyro_filter_data.y=LPButterworth(Pitch_Gyro,&Buffer_OpticalFlow_Gyro[1],&OpticalFlow_Gyro_Parameter)/57.3f;//陀螺仪相位同步角速度
opt_gyro_filter_data.x=OpticalFlow_Rotate_Complementary_Filter(opt_gyro_data.x,gyro_filter_data.x,'x');//使用陀螺仪角速度对光流进行旋转补偿
opt_gyro_filter_data.y=OpticalFlow_Rotate_Complementary_Filter(opt_gyro_data.y,gyro_filter_data.y,'y'); //使用陀螺仪角速度对光流进行旋转补偿