光流

本演示实例以光流例程为例, 演示了如何将数值打包成二进制文件, 并且在C语言端对二进制数值进行解析.

例程说明

该例程提供了OpenMV串口发送光流数据与飞控解析二进制文件的全部代码。

串口发送字符串(ASCALL编码)的方式比较简单, 用C语言的sscanf()语句解析从语法上是完全可以的, 但是在实际工程上测试,解析不是很稳定,易丢数据。 ASCALL编码易出错,缺乏纠错功能。本例程是采用二进制传输的,也就是把数据小数部分乘以一个倍率变成整数,然哦后以C语言的int,short,char的拆分逐8位形式逐位发送。 接收以后先计算校验累加,再重组。这种方式长期使用稳定可靠。

指定分辨率

运行光流代码比对偏移量的时候需要大量的内存,所以我们可选的分辨率范围只有 B64x64B32xB32

获取偏移量

然后我们执行image的find_displacement 函数, 获取上一张图像与当前图像的偏移量

[delta_x, delta_y, response] = old.find_displacement(img)

delta_x : 在x轴方向上的像素偏移量, 如果选用B64x64的话,取值范围为 [-64, 64] delta_y : 在y轴方向上的像素偏移量, 取值范围同上。 response: 可信值/置信值,浮点数,取值范围[0.0, 1.0] 这个值的意思是, 你可以信赖这个偏移量的权值。 如果response下降,也就意味着你就要更怀疑偏移量的真实性。 一般来讲,response 大于0.2,都是ok的。

打包发送数据

判断如果delta_x,delta_y,reponse不为空时,打包数据并通过串口发送。 在send_optical_flow_packet 函数里面我们将数据打包成二进制数据, 添加帧头(用于数据校验)。

这里我需要给大家讲一下struct库,此库的功能就是将Python数据格式与C语言的数据格式通过二进制bit的方式,可以相互转换。在micropython中实现了python标准库中的struct库。

这里用到的是pack函数, 传入的第一个参数<bbiii是整体的数据格式, 其中< 是开始的标志,b代表singed char 有符号字符类型,占一个字节(一个字节8个bit)。i 代表整数类型, 占4个字节。更多数据类型见下方表格, 也可以参照完整版文档 Python3 struct文档

后面参数就是我们要打包并格式化的参数。

0xAA0xAE代表帧头的校验位。

int(x * 100000 ) 这里的x实际上是小数, 但是为了方便之后在C语言中的解析, 我们需要先将小数变换为整数,打包到二进制串中。

后面的y, c亦然。

然后我们调用串口发送该二进制串。

temp = struct.pack("<bbiii",
                       0xAA,
                       0xAE,
                       int(x * 100000 ), # up sample by 4
                       int(y * 100000 ), # up sample by 4
                       int(c * 100000))
Format C Type Python type Standard size Notes
x pad byte no value
c char bytes of length 1 1
b signed char integer 1 (1),(3)
B unsigned char integer 1 (3)
? _Bool bool 1 (1)
h short integer 2 (3)
H unsigned short integer 2 (3)
i int integer 4 (3)
I unsigned int integer 4 (3)
l long integer 4 (3)
L unsigned long integer 4 (3)
q long long integer 8 (2), (3)
Q unsigned long long integer 8 (2), (3)
n ssize_t integer (4)
N size_t integer (4)
e (7) float 2 (5)
f float float 4 (5)
d double float 8 (5)
s char[] bytes
p char[] bytes
P void * integer (6)

C语言获取数据

通过中断的方式在单片机获取到之前我们发送的二进制串。

首先我们获取到前两帧的数据位, 然后将后面的数据读入buffer中。

C语言数据解析

我们以x_offset 为例解释一下其解释教程。

之前我们发送的是4字节的整数, 解析的目的是将这四个字节重新拼接成我们想要的 x_offset 偏移量。

获取的方式就是四个字节分别左移8的倍数(0,1, 2, 3), 然后进行一下或运算

得到的整形参数,做一下变换,变成light飞控中需要的类型与取值范围。

    /* 读取X偏移量原始数据  */
    Optical_flow_org_x = (int)(*(data_buf+1)<<0) | (int)(*(data_buf+2)<<8) | (int)(*(data_buf+3)<<16) | (int)(*(data_buf+4)<<24) ;
    /* 换算到X像素速度  光流像素要结合高度才能算出速度 值范围约+-200.000 */
    Optical_flow_x = (float)((double)Optical_flow_org_x*0.001);

下述tips由 @闭合类时曲线 提供 还有更方便方法是使用C语言中的特性来实现数值变换与提取。 还有个简单一点的解决方法,就是用联合体变量(两变量共享同一内存空间),比如double和uchar[4]联合,发送接受的时候用uchar[],进行运算用double,这样不需要进行转换,只需要调用不同变量名就行。 也可以声明一个double的指针,用强制类型转换指向uchar[4]

源代码

OpenMV发送程序

# MAVLink OpticalFlow Script.
#
# This script sends out OpticalFlow detections using the MAVLink protocol to
# an LIGHT/PixHawk UAV controller for position control using your OpenMV Cam.
#
# P4 = TXD 115200,8,N,1


import sensor, image, time, pyb, struct, math

sensor.reset() # Initialize the camera sensor.
sensor.set_pixformat(sensor.GRAYSCALE) # or sensor.GRAYSCALE
sensor.set_framesize(sensor.B64x64) # or B40x30 or B64x64
clock = time.clock() # Tracks FPS.

old = sensor.snapshot()

uart = pyb.UART(3, 115200, timeout_char = 1000)

def send_optical_flow_packet(x, y, c):
    temp = struct.pack("<bbiii",
                       0xAA,
                       0xAE,
                       int(x * 100000 ), # up sample by 4
                       int(y * 100000 ), # up sample by 4
                       int(c * 100000))
    uart.write(temp)


while(True):
    clock.tick() # 获取时间
    img = sensor.snapshot() # 获取一帧图像
    '''
    find_displacement 这个功能函数使用是是2D FFT算法获得新旧两张图像的相位移动,由于OPenMV上单片机内存的问题,只能计算
    64x64或者64*32的图片(openmv2),如果使用OPenMV3可以计算128*32或者32*128的图片
    '''
    [delta_x, delta_y, response] = old.find_displacement(img) #获取前面一张图像与刚捕获的图像之间的偏移
    old = img.copy()
    #print("%0.6fX   %0.6fY   %0.2fC   %0.2fFPS" % (delta_x, delta_y, response, clock.fps()))
    if (not (math.isnan(delta_x) or math.isnan(delta_y) or math.isnan(response))):
        send_optical_flow_packet(delta_x, delta_y, response)

C语言解析程序

例程解析程序是用于light飞控的,但是不局限于该飞控。

Optical_Flow.h

/* ©2015-2016 Beijing Bechade Opto-Electronic, Co.,Ltd. All rights reserved.
 + 文件名  :Optical_Flow.h
 + 描述    :数据传输头文件
代码尽量做到逐行注释 代码有问题,及时加群交流 作者有偿支持对开源代码的完善 */
#ifndef _OPTICAL_FLOW_H
#define    _OPTICAL_FLOW_H

#include "stm32f4xx.h"

/* 读取X偏移量原始数据  */
extern int Optical_flow_org_x;
/* 读取Y偏移量原始数据 */
extern int Optical_flow_org_y;
/* 读取Confidence原始数据 */
extern int Optical_flow_confidence;

/* 换算到X像素速度  光流像素要结合高度才能算出速度 值范围约+-200.000 */
extern float Optical_flow_x;
/* 换算到Y像素速度  光流像素要结合高度才能算出速度 值范围约+-200.000 */
extern float Optical_flow_y;
/* 读取画面不变率 值范围0-100 */
extern float Optical_flow_con;

/*----------------------------------------------------------
 + 实现功能:数据接收并保存
 + 调用参数:接收到的单字节数据
----------------------------------------------------------*/
extern void Optical_Flow_Receive_Prepare(u8 data);

/*----------------------------------------------------------
 + 实现功能:光流初始化
----------------------------------------------------------*/
extern void Optical_Flow_init();

#endif
/* ©2015-2016 Beijing Bechade Opto-Electronic, Co.,Ltd. All rights reserved. */

Optical_Flow.c

/* ©2015-2016 Beijing Bechade Opto-Electronic, Co.,Ltd. All rights reserved.
 + 文件名  :Optical_Flow.c
 + 描述    :光流数据处理
代码尽量做到逐行注释 代码有问题,及时加群交流 作者有偿支持对开源代码的完善 */
#include "stm32f4xx.h"
#include "Optical_Flow.h"
#include "driver_usart.h"

/* 发送帧头 接收帧头 */
#define title1_received 0xAA
#define title2_received 0xAE

/* 读取X偏移量 读取Y偏移量 读取置信度 */
int Optical_flow_org_x,Optical_flow_org_y,Optical_flow_confidence;
float Optical_flow_x,Optical_flow_y,Optical_flow_con;

/*----------------------------------------------------------
 + 实现功能:数据分析
 + 调用参数:接收到的单字节数据
----------------------------------------------------------*/
void Optical_Flow_Receive_Anl(u8 *data_buf,u8 num)
{
    /* 读取X偏移量原始数据  */
    Optical_flow_org_x = (int)(*(data_buf+1)<<0) | (int)(*(data_buf+2)<<8) | (int)(*(data_buf+3)<<16) | (int)(*(data_buf+4)<<24) ;
    /* 换算到X像素速度  光流像素要结合高度才能算出速度 值范围约+-200.000 */
    Optical_flow_x = (float)((double)Optical_flow_org_x*0.001);

    /* 读取Y偏移量原始数据 */
    Optical_flow_org_y = (int)(*(data_buf+5)<<0) | (int)(*(data_buf+6)<<8) | (int)(*(data_buf+7)<<16) | (int)(*(data_buf+8)<<2 4) ;
    /* 换算到Y像素速度  光流像素要结合高度才能算出速度 值范围约+-200.000 */
    Optical_flow_y = (float)((double)Optical_flow_org_y*0.001);

    /* 读取Confidence原始数据 */
    Optical_flow_confidence = (int)(*(data_buf+9)<<0) | (int)(*(data_buf+10)<<8) | (int)(*(data_buf+11)<<16) | (int)(*(data_buf+12)<<24) ;
    /* 读取画面不变率 值范围0-100 */
    Optical_flow_con = (float)((double)Optical_flow_confidence*0.001);
}

/*----------------------------------------------------------
 + 实现功能:数据接收并保存
 + 调用参数:接收到的单字节数据
----------------------------------------------------------*/
void Optical_Flow_Receive_Prepare(u8 data)
{
    /* 局部静态变量:接收缓存 */
    static u8 RxBuffer[14];
    /* 数据长度 *//* 数据数组下标 */
    static u8  _data_cnt = 0;
    /* 接收状态 */
    static u8 state = 0;

    /* 帧头1 */
    if(state==0&&data==title1_received)
    {
        state=1;
    }
    /* 帧头2 */
    else if(state==1&&data==title2_received)
    {
        state=2;
        _data_cnt = 0;
    }
    /* 接收数据租 */
    else if(state==2)
    {
        RxBuffer[++_data_cnt]=data;
        if(_data_cnt>=12)
        {
            state = 0;
            Optical_Flow_Receive_Anl(RxBuffer,_data_cnt);
        }
    }
    /* 若有错误重新等待接收帧头 */
    else
        state = 0;
}


/*----------------------------------------------------------
 + 实现功能:光流初始化
----------------------------------------------------------*/
void Optical_Flow_init()
{
    /*----------------------------------------------------------
     + 实现功能:串口2初始化
     + 调用参数功能:
     - u32 bound:波特率 38400
     - u8 Priority:中断主优先级 2
     - u8 SubPriority:中断从优先级 0
     - FunctionalState TXenable:发送中断使能 失能
     - FunctionalState RXenable:就收中断使能 使能
    ----------------------------------------------------------*/
    Device_Usart3_ENABLE_Init(115200,2,0,DISABLE,ENABLE);
}

/* ©2015-2016 Beijing Bechade Opto-Electronic, Co.,Ltd. All rights reserved. */
Copyright 杭州云江科技有限公司 2017 all right reserved,powered by Gitbook该文件修订时间: 2018-04-02 09:53:12

results matching ""

    No results matching ""