PX4 FLOW光流应用关键问题讲解 (amobbs.com 阿莫电子技术论坛) -

golaced 发表于 2015-6-12 18:38:35

PX4 FLOW光流应用关键问题讲解

本帖最后由 golaced 于 2015-6-13 09:09 编辑

最近在做新的迷你8轴，想往里面加上光流传感器，淘宝上有许多价格不等有的500多有的800多非常的坑，贵的主要原因就是超声波传感器，但是有些店光传感器就500非常坑和其他店区别就是提供技术支持，但我怀疑估计他们也搞不懂光流算法是什么并且PX4 FLOW光流不直接提供stm32工程文件需要用PX4 console 编译无法在线调试，有时间的人可以把开源文件用keil新建个工程编译下载试试还行不行，楼主暂时还不打算研究光流算法所有就不就行这个工作了。
我买的光流传感器为X宝上最便宜的大概280左右，同时也买了那个很贵的超声波传感器所以也花了500多，但是比那些800，1000的可便宜了不少如果有兴趣可以买网上10元左右的那种超声波传感器更改开源程序，但我觉得还不如用另外的stm32来完成超声波的采集，PX4 FLOW光流中貌似只是多了个卡尔曼滤波器而已，网上那些超声定高的四轴不是也挺好的而且能查到的资料也很多。
刚买来的PX4 FLOW光流自带的是官方程序，效果不怎么好，原来没用过PX4 的产品一开始一头雾水，上网查阅了很多帖子都说效果不好，无意中在某论坛上发现一个人用PX4 FLOW光流定点的视频效果不错还提供固件，于是满怀激动的下载并烧入传感器中实验了一下效果比自带的好了很多这个附件就是网上找到的那个。

第二天再测试就发现坑爹了这个固件测出来的x，y轴数居然一样，我以为是传感器坏了又重新烧了官方程序发现虽然效果不好但是x，y方向是分离的，上网找到了这个提供这个固件的淘宝店询问了回答是要用他们生产的传感器，我就纳闷了本来就是用别人开源的资料，原理图代码都是别人提供的结果自己赚那么多钱，跑到论坛上上传固件吸引了那么多人，我以为是大神造福我们这些菜鸟结果还只适用自己的板子。

我虽然做的东西不多但是在别人东西上改改还是可以的，再次浏览PX4 FLOW光流的资料我认为上面固件好使只不过是调整了一些参数而已，而其他店的传感器输出数据x，y轴一样估计是店主动了点小手脚，PX4 FLOW光流可以调整的参数如下：
Name Default Access Comment
BFLOW_F_THLD 5000 RW This parameter is a feature threashold and limits the quality of patterns that are used to calculate the bottom flow. For low values (e.g. 10) almost every pattern is taken, for higher values (e.g. 100) only significant patters are taken.
BFLOW_V_THLD 30 RW This is a pattern correlation threashold for filtering bad matches. Lower means only strong correlations are accepted.
BFLOW_HIST_FIL 0 RW 1: Flow histogram filter is ON, 0: OFF
BFLOW_GYRO_COM 1 RW 1: Gyro compensation is ON, 0: OFF
BFLOW_LP_FIL 0 RW 1: Lowpass filter on flow output is ON, 0: OFF
BFLOW_W_NEW 0.3 RW Flow lowpass filter gain
DEBUG 1 RW 1: Debug messages ON, 0: OFF
GYRO_SENS_DPS 250 RW Gyroscope sensitivity: 250, 500, 2000 (dps)
GYRO_COMP_THR 0.01 RW Gyro compensation threshold (dps): Gyro data lower than this threshold is not compensated to prevent drift
IMAGE_WIDTH 64 R Image width (pixels)
IMAGE_HEIGHT 64 R Image height (pixels)
IMAGE_L_LIGHT 0 RW 1: Image sensor low light mode ON, 0: OFF
IMAGE_NOISE_C 1 RW 1: Image sensor noise correction ON, 0: OFF
IMAGE_TEST_PAT 0 RW 1: Gray-shaded test pattern mode ON, 0: OFF
LENS_FOCAL_LEN 16 RW Focal length of lens (mm)
POSITION 0 RW 0: Only position 0 is used (Bottom: 0, Front: 1, Top: 2, Back: 3, Right: 4, Left: 5)
SONAR_FILTERED 0 RW 1: Kalman filter on sonar output is ON, 0: OFF
SONAR_KAL_L1 0.8461 RW Sonar Kalman gain L1
SONAR_KAL_L2 6.2034 RW Sonar Kalman gain L2
SYS_ID 81 RW MAVLink System ID
SYS_COMP_ID 50 RW MAVLink Component ID
SYS_SENSOR_ID 77 RW MAVLink Sensor ID
SYS_TYPE 0 RW MAVLink System Type
SYS_AP_TYPE 1 RW MAVLink Autopilot Type
SYS_SW_VER 13XX R Software Version
SYS_SEND_STATE 1 RW 1: Send MAVLink Heartbeat, 0: Not
USART_2_BAUD 115200 R Baudrate USART 2
USART_3_BAUD 115200 R Baudrate USART 3 (Data Output)
USB_SEND_VIDEO 1 RW 1: Send video over USB, 0: Not
USB_SEND_FLOW 1 RW 1: Send flow over USB, 0: Not
USB_SEND_GYRO 1 RW 1: Send gyro data over USB, 0: Not
USB_SEND_FWD 0 RW 1: Send forwarded flow over USB, 0: Not
USB_SEND_DEBUG 1 RW 1: Send debug msgs over USB, 0: Not
VIDEO_RATE 150 RW Time in milliseconds between images of video transmission
VIDEO_ONLY 0 RW 1: High resolution video mode is ON, 0: OFF

其中BFLOW_F_THLD BFLOW_V_THLDBFLOW_W_NEW三个参数对定位效果影响较大，经过楼主尝试得出三个参数为19 200.1时对我的传感器效果较好下面是几个波形：

左右周期运动

匀速运动

上下运动高度波形

这个参数的效果和上面固件的结果基本差不多，如果在经过滤波和加速度融合处理效果估计会更好，当然你也可以自己调整其他参数方法如下pdf所示

我调试出的固件如下：

该固件效果和上面的差不多而且xy分离测量。还有在调整传感器参数是在qground中使用set貌似无法把参数写入rom中，我使用的方法是直接在源码中修改其文件为src/settings.c 每次调试时先连上USB在线修改参数比较结果，满意后就到settings.c中修改对应参数，然后按照上面pdf中方法编译获得固件下载即可。

对于传感器数据接收来说，新手对mavlink那坑爹的协议估计都一头雾水，而且在https://pixhawk.ethz.ch/mavlink/#OPTICAL_FLOW提供的顺序并不是传感器发送帧的顺序，楼主一开始也被这个误导，导致接收到数据缺无法正确解码，正确的顺序应该是：

楼主的接收程序是：

typedef struct
{
uint64_ttime_sec;
u8 id;
int16_t flow_x;
int16_t flow_y;
float flow_comp_x;//Flow in m in x-sensor direction, angular-speed compensated
float flow_comp_y;
u8 quality; //Optical flow quality / confidence. 0: bad, 255: maximum quality
float hight;//ground_distance float Ground distance in m. Positive value: distance known. Negative value: Unknown distance


}FLOW;

FLOW flow;

float ByteToFloat(unsigned char* byteArry)
{
return *((float*)byteArry);
}

u8 FLOW_STATE;
u8 flow_buf;
void FLOW_MAVLINK(unsigned char data)
{
/*
红色的是起始标志位（stx），在v1.0版本中以“FE”作为起始标志。这个标志位在mavlink消息帧接收端进行消息解码时有用处。

第二个格子代表的是灰色部分（payload，称作有效载荷，要用的数据在有效载荷里面）的字节长度（len），范围从0到255之间。在mavlink消息帧接收端可以用它和实际收到的有效载荷的长度比较，以验证有效载荷的长度是否正确。

第三个格子代表的是本次消息帧的序号（seq），每次发完一个消息，这个字节的内容会加1，加到255后会从0重新开始。这个序号用于mavlink消息帧接收端计算消息丢失比例用的，相当于是信号强度。

第四个格子代表了发送本条消息帧的设备的系统编号（sys），使用PIXHAWK刷PX4固件时默认的系统编号为1，用于mavlink消息帧接收端识别是哪个设备发来的消息。

第五个格子代表了发送本条消息帧的设备的单元编号（comp），使用PIXHAWK刷PX4固件时默认的单元编号为50，用于mavlink消息帧接收端识别是设备的哪个单元发来的消息（暂时没什么用）。

第六个格子代表了有效载荷中消息包的编号（msg），注意它和序号是不同的，这个字节很重要，mavlink消息帧接收端要根据这个编号来确定有效载荷里到底放了什么消息包并根据编号选择对应的方式来处理有效载荷里的信息包。
   26*/
// FE 1A| A2 X X| 64

static u8 s_flow=0,data_cnt=0;
u8 cnt_offset=0;
u8 get_one_fame=0;
char floattobyte;
switch(s_flow)
{
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)
{
flow.time_sec=(flow_buf<<64)|(flow_buf<<56)|(flow_buf<<48)|(flow_buf<<40)
|(flow_buf<<32)|(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

}
}

写的不好，也没有校验，不过结果正确，代码里面也包括了串口接收四位数据转换单精度浮点的部分（估计很多新手也很头疼的部分）。

以上内容不敢说全部原创，如有问题望指正。

golaced 发表于 2015-7-22 20:30:42

Simon_起誓发表于 2015-7-22 14:15
成功读取..楼主能解释一下 flow_x，flow_y， flow_comp_x，flow_comp_y具体含义么怎么融合到定点PID里 ...

前两个貌似是像素位移速度后面是融合高度后的真实速度定点apm里貌似用光流输出速度积分换算成了GPS坐标然后给到导航里了你可以看下firmware-master里例子中flow_postion_estimator 那个
/****************************************************************************
*
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
* Author: Samuel Zihlmann <samuezih@ee.ethz.ch>
*    Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/

/**
* @file flow_position_estimator_main.c
*
* Optical flow position estimator
*/

#include <px4_config.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <stdbool.h>
#include <fcntl.h>
#include <float.h>
#include <nuttx/sched.h>
#include <sys/prctl.h>
#include <drivers/drv_hrt.h>
#include <termios.h>
#include <errno.h>
#include <limits.h>
#include <math.h>
#include <uORB/uORB.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/filtered_bottom_flow.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include <poll.h>
#include <platforms/px4_defines.h>

#include "flow_position_estimator_params.h"

__EXPORT int flow_position_estimator_main(int argc, char *argv[]);
static bool thread_should_exit = false; /**< Daemon exit flag */
static bool thread_running = false; /**< Daemon status flag */
static int daemon_task; /**< Handle of daemon task / thread */

int flow_position_estimator_thread_main(int argc, char *argv[]);
static void usage(const char *reason);

static void usage(const char *reason)
{
if (reason) {
fprintf(stderr, "%s\n", reason);
}

fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n");
exit(1);
}

/**
* The daemon app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to px4_task_spawn_cmd().
*/
int flow_position_estimator_main(int argc, char *argv[])
{
if (argc < 2) {
usage("missing command");
}

if (!strcmp(argv, "start")) {
if (thread_running) {
printf("flow position estimator already running\n");
/* this is not an error */
exit(0);
}

thread_should_exit = false;
daemon_task = px4_task_spawn_cmd("flow_position_estimator",
      SCHED_DEFAULT,
      SCHED_PRIORITY_MAX - 5,
      4000,
      flow_position_estimator_thread_main,
      (argv) ? (char * const *)&argv : (char * const *)NULL);
exit(0);
}

if (!strcmp(argv, "stop")) {
thread_should_exit = true;
exit(0);
}

if (!strcmp(argv, "status")) {
if (thread_running) {
printf("\tflow position estimator is running\n");

} else {
printf("\tflow position estimator not started\n");
}

exit(0);
}

usage("unrecognized command");
exit(1);
}

int flow_position_estimator_thread_main(int argc, char *argv[])
{
/* welcome user */
thread_running = true;
printf(" starting\n");

/* rotation matrix for transformation of optical flow speed vectors */
static const int8_t rotM_flow_sensor = {{0, -1, 0 },
{ 1, 0, 0 },
{0, 0, 1 }
}; // 90deg rotated
const float time_scale = powf(10.0f, -6.0f);
static float speed = {0.0f, 0.0f, 0.0f};
static float flow_speed = {0.0f, 0.0f, 0.0f};
static float global_speed = {0.0f, 0.0f, 0.0f};
static uint32_t counter = 0;
static uint64_t time_last_flow = 0; // in ms
static float dt = 0.0f; // seconds
static float sonar_last = 0.0f;
static bool sonar_valid = false;
static float sonar_lp = 0.0f;

/* subscribe to vehicle status, attitude, sensors and flow*/
struct actuator_armed_s armed;
memset(&armed, 0, sizeof(armed));
struct vehicle_control_mode_s control_mode;
memset(&control_mode, 0, sizeof(control_mode));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_attitude_setpoint_s att_sp;
memset(&att_sp, 0, sizeof(att_sp));
struct optical_flow_s flow;
memset(&flow, 0, sizeof(flow));

/* subscribe to parameter changes */
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));

/* subscribe to armed topic */
int armed_sub = orb_subscribe(ORB_ID(actuator_armed));

/* subscribe to safety topic */
int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));

/* subscribe to attitude */
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));

/* subscribe to attitude setpoint */
int vehicle_attitude_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));

/* subscribe to optical flow*/
int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));

/* init local position and filtered flow struct */
struct vehicle_local_position_s local_pos = {
.x = 0.0f,
.y = 0.0f,
.z = 0.0f,
.vx = 0.0f,
.vy = 0.0f,
.vz = 0.0f
};
struct filtered_bottom_flow_s filtered_flow = {
.sumx = 0.0f,
.sumy = 0.0f,
.vx = 0.0f,
.vy = 0.0f
};

/* advert pub messages */
orb_advert_t local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos);
orb_advert_t filtered_flow_pub = orb_advertise(ORB_ID(filtered_bottom_flow), &filtered_flow);

/* vehicle flying status parameters */
bool vehicle_liftoff = false;
bool sensors_ready = false;

/* parameters init*/
struct flow_position_estimator_params params;
struct flow_position_estimator_param_handles param_handles;
parameters_init(&param_handles);
parameters_update(&param_handles, &params);

perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "flow_position_estimator_runtime");
perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "flow_position_estimator_interval");
perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_position_estimator_err");

while (!thread_should_exit) {

if (sensors_ready) {
/*This runs at the rate of the sensors */
struct pollfd fds = {
{ .fd = optical_flow_sub, .events = POLLIN },
{ .fd = parameter_update_sub, .events = POLLIN }
};

/* wait for a sensor update, check for exit condition every 500 ms */
int ret = poll(fds, 2, 500);

if (ret < 0) {
/* poll error, count it in perf */
perf_count(mc_err_perf);

} else if (ret == 0) {
/* no return value, ignore */
// printf(" no bottom flow.\n");
} else {

/* parameter update available? */
if (fds.revents & POLLIN) {
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);

parameters_update(&param_handles, &params);
printf(" parameters updated.\n");
}

/* only if flow data changed */
if (fds.revents & POLLIN) {
perf_begin(mc_loop_perf);

orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);
/* got flow, updating attitude and status as well */
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp);
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);

/* vehicle state estimation */
float sonar_new = flow.ground_distance_m;

/* set liftoff boolean
* -> at bottom sonar sometimes does not work correctly, and has to be calibrated (distance higher than 0.3m)
* -> accept sonar measurements after reaching calibration distance (values between 0.3m and 1.0m for some time)
* -> minimum sonar value 0.3m
*/

if (!vehicle_liftoff) {
if (armed.armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f) {
vehicle_liftoff = true;
}

} else {
if (!armed.armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f)) {
vehicle_liftoff = false;
}
}

/* calc dt between flow timestamps */
/* ignore first flow msg */
if (time_last_flow == 0) {
time_last_flow = flow.timestamp;
continue;
}

dt = (float)(flow.timestamp - time_last_flow) * time_scale ;
time_last_flow = flow.timestamp;

/* only make position update if vehicle is lift off or DEBUG is activated*/
if (vehicle_liftoff || params.debug) {
/* copy flow */
if (flow.integration_timespan > 0) {
flow_speed = flow.pixel_flow_x_integral / (flow.integration_timespan / 1e6f) * flow.ground_distance_m;
flow_speed = flow.pixel_flow_y_integral / (flow.integration_timespan / 1e6f) * flow.ground_distance_m;

} else {
flow_speed = 0;
flow_speed = 0;
}

flow_speed = 0.0f;

/* convert to bodyframe velocity */
for (uint8_t i = 0; i < 3; i++) {
float sum = 0.0f;

for (uint8_t j = 0; j < 3; j++) {
sum = sum + flow_speed * rotM_flow_sensor;
}

speed = sum;
}

/* update filtered flow */
filtered_flow.sumx = filtered_flow.sumx + speed * dt;
filtered_flow.sumy = filtered_flow.sumy + speed * dt;
filtered_flow.vx = speed;
filtered_flow.vy = speed;

// TODO add yaw rotation correction (with distance to vehicle zero)

/* convert to globalframe velocity
* -> local position is currently not used for position control
*/
for (uint8_t i = 0; i < 3; i++) {
float sum = 0.0f;

for (uint8_t j = 0; j < 3; j++) {
sum = sum + speed * PX4_R(att.R, i, j);
}

global_speed = sum;
}

local_pos.x = local_pos.x + global_speed * dt;
local_pos.y = local_pos.y + global_speed * dt;
local_pos.vx = global_speed;
local_pos.vy = global_speed;
local_pos.xy_valid = true;
local_pos.v_xy_valid = true;

} else {
/* set speed to zero and let position as it is */
filtered_flow.vx = 0;
filtered_flow.vy = 0;
local_pos.vx = 0;
local_pos.vy = 0;
local_pos.xy_valid = false;
local_pos.v_xy_valid = false;
}

/* filtering ground distance */
if (!vehicle_liftoff || !armed.armed) {
/* not possible to fly */
sonar_valid = false;
local_pos.z = 0.0f;
local_pos.z_valid = false;

} else {
sonar_valid = true;
}

if (sonar_valid || params.debug) {
/* simple lowpass sonar filtering */
/* if new value or with sonar update frequency */
if (sonar_new != sonar_last || counter % 10 == 0) {
sonar_lp = 0.05f * sonar_new + 0.95f * sonar_lp;
sonar_last = sonar_new;
}

float height_diff = sonar_new - sonar_lp;

/* if over 1/2m spike follow lowpass */
if (height_diff < -params.sonar_lower_lp_threshold || height_diff > params.sonar_upper_lp_threshold) {
local_pos.z = -sonar_lp;

} else {
local_pos.z = -sonar_new;
}

local_pos.z_valid = true;
}

filtered_flow.timestamp = hrt_absolute_time();
local_pos.timestamp = hrt_absolute_time();

/* publish local position */
if (isfinite(local_pos.x) && isfinite(local_pos.y) && isfinite(local_pos.z)
   && isfinite(local_pos.vx) && isfinite(local_pos.vy)) {
orb_publish(ORB_ID(vehicle_local_position), local_pos_pub, &local_pos);
}

/* publish filtered flow */
if (isfinite(filtered_flow.sumx) && isfinite(filtered_flow.sumy) && isfinite(filtered_flow.vx)
   && isfinite(filtered_flow.vy)) {
orb_publish(ORB_ID(filtered_bottom_flow), filtered_flow_pub, &filtered_flow);
}

/* measure in what intervals the position estimator runs */
perf_count(mc_interval_perf);
perf_end(mc_loop_perf);

}
}

} else {
/* sensors not ready waiting for first attitude msg */

/* polling */
struct pollfd fds = {
{ .fd = vehicle_attitude_sub, .events = POLLIN },
};

/* wait for a attitude message, check for exit condition every 5 s */
int ret = poll(fds, 1, 5000);

if (ret < 0) {
/* poll error, count it in perf */
perf_count(mc_err_perf);

} else if (ret == 0) {
/* no return value, ignore */
printf(" no attitude received.\n");

} else {
if (fds.revents & POLLIN) {
sensors_ready = true;
printf(" initialized.\n");
}
}
}

counter++;
}

printf(" exiting.\n");
thread_running = false;

close(vehicle_attitude_setpoint_sub);
close(vehicle_attitude_sub);
close(armed_sub);
close(control_mode_sub);
close(parameter_update_sub);
close(optical_flow_sub);

perf_print_counter(mc_loop_perf);
perf_free(mc_loop_perf);

fflush(stdout);
return 0;
}

golaced 发表于 2015-7-29 13:12:29

更新RAD帧读取：

FLOW flow;
FLOW_RAD flow_rad;
FLOW_FIX flow_fix;
u8 FLOW_STATE;
u8 flow_buf;
u8 flow_buf_rad;
float ByteToFloat(unsigned char* byteArry)
{
return *((float*)byteArry);
}

#defineIIR_ORDER 4    //使用IIR滤波器的阶数
static double b_IIR ={0.0008f, 0.0032f, 0.0048f, 0.0032f, 0.0008f};//系数b
static double a_IIR ={1.0000f, -3.0176f, 3.5072f, -1.8476f, 0.3708f};//系数a
static double InPut_IIR = {0};
static double OutPut_IIR = {0};

void FLOW_MAVLINK(unsigned char data)
{
/*
红色的是起始标志位（stx），在v1.0版本中以“FE”作为起始标志。这个标志位在mavlink消息帧接收端进行消息解码时有用处。

第二个格子代表的是灰色部分（payload，称作有效载荷，要用的数据在有效载荷里面）的字节长度（len），范围从0到255之间。在mavlink消息帧接收端可以用它和实际收到的有效载荷的长度比较，以验证有效载荷的长度是否正确。

第三个格子代表的是本次消息帧的序号（seq），每次发完一个消息，这个字节的内容会加1，加到255后会从0重新开始。这个序号用于mavlink消息帧接收端计算消息丢失比例用的，相当于是信号强度。

第四个格子代表了发送本条消息帧的设备的系统编号（sys），使用PIXHAWK刷PX4固件时默认的系统编号为1，用于mavlink消息帧接收端识别是哪个设备发来的消息。

第五个格子代表了发送本条消息帧的设备的单元编号（comp），使用PIXHAWK刷PX4固件时默认的单元编号为50，用于mavlink消息帧接收端识别是设备的哪个单元发来的消息（暂时没什么用）。

第六个格子代表了有效载荷中消息包的编号（msg），注意它和序号是不同的，这个字节很重要，mavlink消息帧接收端要根据这个编号来确定有效载荷里到底放了什么消息包并根据编号选择对应的方式来处理有效载荷里的信息包。
   26*/
// FE 1A| A2 X X| 64

static u8 s_flow=0,data_cnt=0;
u8 cnt_offset=0;
u8 get_one_fame=0;
char floattobyte;
switch(s_flow)
{
case 0: if(data==0xFE)
s_flow=1;
break;
case 1: if(data==0x1A||data==0x2C)
{ 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 if(FLOW_STATE==106){
if(data_cnt<44)
{s_flow=3; flow_buf_rad=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)
{
if(FLOW_STATE==100){
flow.time_sec=(flow_buf<<64)|(flow_buf<<56)|(flow_buf<<48)|(flow_buf<<40)
|(flow_buf<<32)|(flow_buf<<16)|(flow_buf<<8)|(flow_buf);
floattobyte=flow_buf;
floattobyte=flow_buf;
floattobyte=flow_buf;
floattobyte=flow_buf;
flow.flow_comp_x.originf =ByteToFloat(floattobyte);
floattobyte=flow_buf;
floattobyte=flow_buf;
floattobyte=flow_buf;
floattobyte=flow_buf;
flow.flow_comp_y.originf =ByteToFloat(floattobyte);
floattobyte=flow_buf;
floattobyte=flow_buf;
floattobyte=flow_buf;
floattobyte=flow_buf;
   flow.hight.originf=ByteToFloat(floattobyte);//ground_distance float Ground distance in m. Positive value: distance known. Negative value: Unknown distance
   flow.flow_x.origin=(int16_t)((flow_buf)|(flow_buf<<8));
   flow.flow_y.origin=(int16_t)((flow_buf)|(flow_buf<<8));
flow.id=flow_buf;
   flow.quality=flow_buf; //Optical flow quality / confidence. 0: bad, 255: maximum quality
flow_fix.flow_x.average = IIR_I_Filter(flow_fix.flow_x.origin , InPut_IIR, OutPut_IIR, b_IIR, IIR_ORDER+1, a_IIR, IIR_ORDER+1);
flow.flow_y.average = IIR_I_Filter(flow.flow_y.origin , InPut_IIR, OutPut_IIR, b_IIR, IIR_ORDER+1, a_IIR, IIR_ORDER+1);
flow.flow_comp_x.average = IIR_I_Filter(flow.flow_comp_x.originf , InPut_IIR, OutPut_IIR, b_IIR, IIR_ORDER+1, a_IIR, IIR_ORDER+1);
flow.flow_comp_y.average = IIR_I_Filter(flow.flow_comp_y.originf , InPut_IIR, OutPut_IIR, b_IIR, IIR_ORDER+1, a_IIR, IIR_ORDER+1);
}
else if(FLOW_STATE==106)
{
   flow_rad.time_usec=(flow_buf_rad<<64)|(flow_buf_rad<<56)|(flow_buf_rad<<48)|(flow_buf_rad<<40)
|(flow_buf_rad<<32)|(flow_buf_rad<<16)|(flow_buf_rad<<8)|(flow_buf_rad);
flow_rad.integration_time_us=(flow_buf_rad<<32)|(flow_buf_rad<<16)|(flow_buf_rad<<8)|(flow_buf_rad);
floattobyte=flow_buf_rad;
floattobyte=flow_buf_rad;
floattobyte=flow_buf_rad;
floattobyte=flow_buf_rad;
flow_rad.integrated_x=ByteToFloat(floattobyte);
floattobyte=flow_buf_rad;
floattobyte=flow_buf_rad;
floattobyte=flow_buf_rad;
floattobyte=flow_buf_rad;
flow_rad.integrated_y=ByteToFloat(floattobyte);
floattobyte=flow_buf_rad;
floattobyte=flow_buf_rad;
floattobyte=flow_buf_rad;
floattobyte=flow_buf_rad;
flow_rad.integrated_xgyro=ByteToFloat(floattobyte);
floattobyte=flow_buf_rad;
floattobyte=flow_buf_rad;
floattobyte=flow_buf_rad;
floattobyte=flow_buf_rad;
flow_rad.integrated_ygyro=ByteToFloat(floattobyte);
floattobyte=flow_buf_rad;
floattobyte=flow_buf_rad;
floattobyte=flow_buf_rad;
floattobyte=flow_buf_rad;
flow_rad.integrated_zgyro=ByteToFloat(floattobyte);
flow_rad.time_delta_distance_us=(flow_buf_rad<<32)|(flow_buf_rad<<16)|(flow_buf_rad<<8)|(flow_buf_rad);
floattobyte=flow_buf_rad;
floattobyte=flow_buf_rad;
floattobyte=flow_buf_rad;
floattobyte=flow_buf_rad;
flow_rad.distance=ByteToFloat(floattobyte);
flow_rad.temperature=(flow_buf_rad<<8)|(flow_buf_rad);
flow_rad.sensor_id=(flow_buf_rad);
flow_rad.quality==(flow_buf_rad);

flow_fix.flow_x.origin =flow.flow_x.origin + flow_rad.integrated_ygyro*flow_fix.scale_rad_fix;
flow_fix.flow_y.origin =flow.flow_y.origin - flow_rad.integrated_xgyro*flow_fix.scale_rad_fix;
flow_fix.flow_comp_x.originf =flow.flow_comp_x.originf - flow_rad.integrated_ygyro*flow_fix.scale_rad_fix_comp;
flow_fix.flow_comp_y.originf =flow.flow_comp_y.originf + flow_rad.integrated_xgyro*flow_fix.scale_rad_fix_comp;
}
}
}

头文件：
typedef struct
{
float average;//Flow in m in x-sensor direction, angular-speed compensated
float originf;
int16_t origin;
}FLOW_DATA;

typedef struct
{
uint64_ttime_sec;
u8 id;
FLOW_DATA flow_x;
FLOW_DATA flow_y;
FLOW_DATA flow_comp_x;//Flow in m in x-sensor direction, angular-speed compensated
FLOW_DATA flow_comp_y;
u8 quality; //Optical flow quality / confidence. 0: bad, 255: maximum quality
FLOW_DATA hight;//ground_distance float Ground distance in m. Positive value: distance known. Negative value: Unknown distance
}FLOW;

typedef struct
{
uint64_t time_usec; ///< Timestamp (microseconds, synced to UNIX time or since system boot)
uint32_t integration_time_us; ///< Integration time in microseconds. Divide integrated_x and integrated_y by the integration time to obtain average flow. The integration time also indicates the.
float integrated_x; ///< Flow in radians around X axis (Sensor RH rotation about the X axis induces a positive flow. Sensor linear motion along the positive Y axis induces a negative flow.)
float integrated_y; ///< Flow in radians around Y axis (Sensor RH rotation about the Y axis induces a positive flow. Sensor linear motion along the positive X axis induces a positive flow.)
float integrated_xgyro; ///< RH rotation around X axis (rad)
float integrated_ygyro; ///< RH rotation around Y axis (rad)
float integrated_zgyro; ///< RH rotation around Z axis (rad)
uint32_t time_delta_distance_us; ///< Time in microseconds since the distance was sampled.
float distance; ///< Distance to the center of the flow field in meters. Positive value (including zero): distance known. Negative value: Unknown distance.
int16_t temperature; ///< Temperature * 100 in centi-degrees Celsius
uint8_t sensor_id; ///< Sensor ID
uint8_t quality; ///< Optical flow quality / confidence. 0: no valid flow, 255: maximum quality
}FLOW_RAD;

typedef struct
{
FLOW_DATA flow_x;
FLOW_DATA flow_y;
FLOW_DATA flow_comp_x;//Flow in m in x-sensor direction, angular-speed compensated
FLOW_DATA flow_comp_y;
float scale_rad_fix;
float scale_rad_fix_comp;
FLOW_DATA hight;//ground_distance float Ground distance in m. Positive value: distance known. Negative value: Unknown distance
}FLOW_FIX;

golaced 发表于 2017-5-23 16:09:31

新贴连接https://www.amobbs.com/thread-5673935-1-1.html 提供px4flow 光流串口接受和预处理库函数头文件里有调用方法

hemeizhi 发表于 2015-6-12 18:50:54

赞一个，支持

Puppey 发表于 2015-6-12 19:21:05

写的不错，就是字体太大了！

liyang53719 发表于 2015-6-12 19:25:58

楼主牛人，支持楼主，打击JS！

abutter 发表于 2015-6-12 19:29:58

自己赶紧改一下吧，看起来太费劲了。

yuxiang2 发表于 2015-6-12 20:54:43

不错的帖子

quner 发表于 2015-6-12 21:04:55

字体太大，改改吧

yuwangwangyu 发表于 2015-6-12 21:55:25

我已经移植到MDK下了{:loveliness:}

本质发表于 2015-6-12 23:18:10

我也在入门，你是用PX4带的eclipse开发的吗

golaced 发表于 2015-6-13 00:12:22

本质发表于 2015-6-12 23:18
我也在入门，你是用PX4带的eclipse开发的吗

没有我还没对那个有接触

dswkl11 发表于 2015-6-13 10:24:15

PX4FLOW-FX版光流传感器，好像很不错，就是不知道有多精准

Halwking 发表于 2015-6-14 12:20:55

正准备用PX4 FLOW，好东西

fengshao1370 发表于 2015-6-14 21:43:10

楼主研究的很深入了，我一开始买了个开源固件的光流，但是效果真的好差，后来又买了一个应该是你说的那家动了手脚的，主要是急着用就没有研究，现在是用串口接收，楼主用I2C实现了吗？我看I2C很简单，但是不行，不就是先写地址0X42，再写0，再读数据吗？

面向大海 发表于 2015-6-14 23:10:39

楼主研究PixHawk飞控吗？看了好久现在还没太整明白飞控的程序架构想做二次开发，可迟迟不能上手

Hgww_MKV 发表于 2015-6-15 00:27:20

我估计是固件的问题，刷了继续

Hgww_MKV 发表于 2015-6-15 00:31:40

fengshao1370 发表于 2015-6-14 21:43
楼主研究的很深入了，我一开始买了个开源固件的光流，但是效果真的好差，后来又买了一个应该是你说的那家动 ...

0X42是读的地址，他是7位地址。末尾代表读或者写。给0X43那就是向SLAVE（从）里面写数据了。表示没弄出来

Hgww_MKV 发表于 2015-6-15 00:50:41

if(FLOW_STATE=100){
if(data_cnt<26)这里是写错了吧。。。该是if(FLOW_STATE==100)吧。。。

golaced 发表于 2015-6-15 10:32:38

面向大海发表于 2015-6-14 23:10
楼主研究PixHawk飞控吗？看了好久现在还没太整明白飞控的程序架构想做二次开发，可迟迟不能上手 ...

我没用过都是自己尝试开发但是做的也不好

golaced 发表于 2015-6-15 10:42:04

fengshao1370 发表于 2015-6-14 21:43
楼主研究的很深入了，我一开始买了个开源固件的光流，但是效果真的好差，后来又买了一个应该是你说的那家动 ...

没用i2c 板子上串口多用起来方便

golaced 发表于 2015-6-15 10:42:45

Hgww_MKV 发表于 2015-6-15 00:50
if(FLOW_STATE=100){
if(data_cnt

是的是的是的是的是的是的是的是的是的

aptd 发表于 2015-6-15 10:43:47

楼主好人，有时间看看

shenzihao6 发表于 2015-6-15 10:45:21

楼主绝对是牛人

snow_blow 发表于 2015-6-15 10:52:42

楼主好人
好帖留名。

golaced 发表于 2015-6-15 11:06:00

通过光流测试波形可以看到，光流传感器非常容易受外界干扰，传感器上下移动，倾斜和左右旋转都会造成读数大幅移动，原因第一是本身图形的变化第二是光流算法中用到了陀螺仪读数和高度对图像光流的变化进行的修正，因此我感觉光流传感器只能用于定点悬停，想超过惯性导航无GPS下自主巡航貌似不太靠谱，需要一套非常完整的机制来确定什么时候该用光流传感器输出的速度和对特殊情况下的（光流数据容易受干扰）的滤波

Queeniee 发表于 2015-6-18 17:36:02

楼主威武！！！正在调

Hgww_MKV 发表于 2015-6-19 12:53:05

楼主我用你的固件和官方的固件没有 FLOW_m_COMP_x，和 FLOW_m_COMP_x输出只有FLOW_X 和FLOW_y输出。不知道是哪里没有使能还是怎么的。楼主的光流能给个图片看看吗

golaced 发表于 2015-6-19 16:34:42

Hgww_MKV 发表于 2015-6-19 12:53
楼主我用你的固件和官方的固件没有 FLOW_m_COMP_x，和 FLOW_m_COMP_x输出只有FLOW_X 和FLOW_y输出。不 ...

你如果没装超声模块没有那个数据是正常的因为光流算法中用到高度值来计算位移速度没有高度值只能输出图形中光流像素的位移量你可以参考源码在主控制器外围添加淘宝上便宜的传感器计算出速度

Hgww_MKV 发表于 2015-6-24 22:05:43

楼主的固件刷新率只有10到20HZ完全不能满足需要啊{:shutup:}

qiguibao 发表于 2015-6-27 08:59:10

刷官方固件，用楼主参数，不行呀

TheBest 发表于 2015-7-3 20:01:55

{:cry:}感谢楼主分享啊，本来都快对光流放弃了，现在感觉又有阳光了

TheBest 发表于 2015-7-3 20:03:06

还有我想问下楼主这个px4flow怎么下程序那，用串口的mcuisp下吗

TheBest 发表于 2015-7-3 20:39:53

呵呵，找到哦怎么修改了，跟改apm参数差不多，逗比了。

quzhanguang3 发表于 2015-7-10 16:54:59

请问左右周期运动是指左右平移还是原地左右转动？我尝试了下左右平移时波形变化不大，左右转动倒是波形和楼主差不多

nongxiaoming 发表于 2015-7-10 17:11:52

我已经把光流的代码移植到MDK下面了，并做了优化

golaced 发表于 2015-7-11 10:47:27

quzhanguang3 发表于 2015-7-10 16:54
请问左右周期运动是指左右平移还是原地左右转动？我尝试了下左右平移时波形变化不大，左右转动倒是波形和楼 ...

平移平移平移平移

golaced 发表于 2015-7-11 10:47:50

nongxiaoming 发表于 2015-7-10 17:11
我已经把光流的代码移植到MDK下面了，并做了优化

能分享下工程吗

wq13758145090 发表于 2015-7-11 17:36:30

虽然看不懂。但是觉得很厉害。。

开放实验室小组 发表于 2015-7-12 00:02:08

楼主，刚上手这个模块想请问下光流用作定点悬浮应该大概是一个怎么样的思路？能从里面读出flow_x和flow_y的读数，但从qground看波形扰动太大了…真的能做定点悬浮吗？那个平移距离应该怎么算？谢谢赐教！

golaced 发表于 2015-7-15 09:29:27

开放实验室小组发表于 2015-7-12 00:02
楼主，刚上手这个模块想请问下光流用作定点悬浮应该大概是一个怎么样的思路？能从里面读出flow_x和flow_y的 ...

我还没弄，要悬停先要消除左右倾斜时也会输出速度，我开启了模块里的角度修正但是没什么卵子用，如果你给角度控制量限幅的很小然后只在悬停状态来使用应该直接用光流输出速度做反馈加上一个速度闭环就行，但是我想还是把光流像素位移量读出来自己做修正计算速度具体方法可以参照：http://copter.ardupilot.cn/wiki/optical-flow-sensor/#_APM25

yangchang66 发表于 2015-7-16 14:27:50

楼主你好，我是新手刚学习px4flow模块，用你的固件可以读出flow_x和flow_y，可以分享下楼主的工程吗？想修改一下固件代码

yangchang66 发表于 2015-7-18 02:19:56

楼主你好，我用你的px4flow接收程序可以读出你的固件里面flow_x和flow_y的值但是却读不出官方固件的值，想问一下楼主是改了源程序里面的发送格式吗，可是我对着看的官方程序的发送格式和你的接收程序是对应的啊，谢谢楼主指点

golaced 发表于 2015-7-18 15:56:55

yangchang66 发表于 2015-7-18 02:19
楼主你好，我用你的px4flow接收程序可以读出你的固件里面flow_x和flow_y的值但是却读不出官方固件的值，想 ...

没改读不出哪个值

yangchang66 发表于 2015-7-18 16:34:23

golaced 发表于 2015-7-18 15:56
没改读不出哪个值

就是用你的px4flow接收程序读不出官方固件的flow_x和flow_y值，但是可以读出你的固件里面flow_x和flow_y的值，是因为传感器参数的原因吗

golaced 发表于 2015-7-18 22:25:16

yangchang66 发表于 2015-7-18 16:34
就是用你的px4flow接收程序读不出官方固件的flow_x和flow_y值，但是可以读出你的固件里面flow_x和flow_y ...

在v1.0版本中以“FE”作为起始标志不知道你的版本是什么的V0.9是0x55

yangchang66 发表于 2015-7-19 23:55:08

golaced 发表于 2015-7-18 22:25
在v1.0版本中以“FE”作为起始标志不知道你的版本是什么的V0.9是0x55

谢谢楼主了，我们找到原因了，是因为参数给的不对导致接收的数据有问题，换了楼主的参数就可以接收数据了，现在正在修改超声波代码，准备装一个廉价超声波上去。

Simon_起誓 发表于 2015-7-20 15:44:11

楼主的飞控板是什么芯片《光流和飞控是用串口通信么？

golaced 发表于 2015-7-20 18:40:20

Simon_起誓发表于 2015-7-20 15:44
楼主的飞控板是什么芯片《光流和飞控是用串口通信么？

我的飞控是stm32f407Vgt 是串口通信

Simon_起誓 发表于 2015-7-20 19:49:37

golaced 发表于 2015-7-20 18:40
我的飞控是stm32f407Vgt 是串口通信

171215925@qq.com串口通信的.c文件能给我参考一下么{:smile:} 本人学生

hemeizhi 发表于 2015-7-21 09:57:04

up！！！！

golaced 发表于 2015-7-21 15:00:11

Simon_起誓发表于 2015-7-20 19:49
串口通信的.c文件能给我参考一下么本人学生

前面有代码

Simon_起誓 发表于 2015-7-22 14:15:07

成功读取..楼主能解释一下 flow_x，flow_y， flow_comp_x，flow_comp_y具体含义么怎么融合到定点PID里

cdut 发表于 2015-7-25 15:33:24

楼主试过用I2C读取数据吗？还有就是我买的不带超声波的版本，我准备外接一个便宜超声波传感器。然后再飞控里面来计算真是移动速度。摄像头焦距是16mm吧，我看到一个文献说的，真实移动速度=光流像素速度X（高度/焦距）。具体算法是什么呢

正十七 发表于 2015-7-27 18:10:34

楼主你好，我用的这个软件V2.60的，怎么把你的固件刷进去看看效果呀

qidian 发表于 2015-7-29 11:30:15

学习了，谢谢楼主

golaced 发表于 2015-7-29 13:11:24

正十七发表于 2015-7-27 18:10
楼主你好，我用的这个软件V2.60的，怎么把你的固件刷进去看看效果呀

setup里的固件更新

jssk01 发表于 2015-7-29 13:13:59

楼主，你编译px4flow代码的环境是什么？

golaced 发表于 2015-7-29 13:20:57

jssk01 发表于 2015-7-29 13:13
楼主，你编译px4flow代码的环境是什么？

我还没编译过只是那模块直接用

golaced 发表于 2015-7-29 13:22:25

cdut 发表于 2015-7-25 15:33
楼主试过用I2C读取数据吗？还有就是我买的不带超声波的版本，我准备外接一个便宜超声波传感器。然后再飞控 ...

FLOWsrc 文件下 main。c里有像素换算速度的部分
const float focal_length_px = (12) / (4.0f * 6.0f) * 1000.0f; //original focal lenght: 12mm pixelsize: 6um, binning 4 enabled
flow_compx = flow_fix.flow_x.origin / focal_length_px / (flow_rad.integration_time_us / 1000000.0f);

yangchang66 发表于 2015-7-29 23:21:36

楼主的滤波处理函数IIR_I_Filter可以参考一下吗，谢谢

golaced 发表于 2015-7-30 14:26:10

yangchang66 发表于 2015-7-29 23:21
楼主的滤波处理函数IIR_I_Filter可以参考一下吗，谢谢

/*====================================================================================================*/
/*====================================================================================================*
** 函数名称: IIR_I_Filter
** 功能描述: IIR直接I型滤波器
** 输入: InData 为当前数据
**       *x 储存未滤波的数据
**       *y 储存滤波后的数据
**       *b 储存系数b
**       *a 储存系数a
**       nb 数组*b的长度
**       na 数组*a的长度
**       LpfFactor
** 输出: OutData
** 说明: 无
** 函数原型: y(n) = b0*x(n) + b1*x(n-1) + b2*x(n-2) -
               a1*y(n-1) - a2*y(n-2)
**====================================================================================================*/
/*====================================================================================================*/
double IIR_I_Filter(double InData, double *x, double *y, double *b, short nb, double *a, short na)
{
double z1,z2;
short i;
double OutData;

for(i=nb-1; i>0; i--)
{
x=x;
}

x = InData;

for(z1=0,i=0; i<nb; i++)
{
z1 += x*b;
}

for(i=na-1; i>0; i--)
{
y=y;
}

for(z2=0,i=1; i<na; i++)
{
z2 += y*a;
}

y = z1 - z2;
OutData = y;

return OutData;
}

ohyeahh 发表于 2015-8-1 17:36:35

楼主的光流固件效果不错

勤奋的MCU 发表于 2015-8-3 20:32:41

开放实验室小组发表于 2015-7-12 00:02
楼主，刚上手这个模块想请问下光流用作定点悬浮应该大概是一个怎么样的思路？能从里面读出flow_x和flow_y的 ...

你好，请问你的光流能实现悬停了么？效果怎么样呢？

勤奋的MCU 发表于 2015-8-3 23:16:38

ohyeahh 发表于 2015-8-1 17:36
楼主的光流固件效果不错

你好，能交流一下光流使用心得么？刚上手，望指导，感谢！594250997@qq.com

勤奋的MCU 发表于 2015-8-4 03:08:54

楼主，你好，请问参考手册中的《PX4、
PIXhawk（PIXRaptor）编译环境的建立及源码的下载编译》一文，百度了，没找到，在哪可以找到啊？如何打开源代码进行编译啊？

golaced 发表于 2015-8-4 21:48:38

勤奋的MCU 发表于 2015-8-4 03:08
楼主，你好，请问参考手册中的《PX4、
PIXhawk（PIXRaptor）编译环境的建立及源码的下载编译》一文，百度了 ...

我还没试过

longjh 发表于 2015-8-4 22:22:23

不知道发送出去的数据是什么数据，i2c读取的地址不知道怎么写

勤奋的MCU 发表于 2015-8-5 01:11:44

golaced 发表于 2015-8-4 21:48
我还没试过

楼主，请问你程序里面的32的采样频率和CMOS的刷新频率是多少啊？

勤奋的MCU 发表于 2015-8-5 01:24:51

golaced 发表于 2015-8-4 21:48
我还没试过

还有7天前更新的更新RAD帧读取里，关于IIR滤波的程序，固件里面是没有的吧？还有那个超声波与PX4FLOW是怎么连接的啊？感谢楼主，不吝赐教！

勤奋的MCU 发表于 2015-8-8 12:47:26

楼主，你更新的RAD帧读取的那段代码，后半段的代码程序里面读不出任何值。。。只有你最开始的代码读出了和你一样的值。。。但是看波形。。左右平移的时候波形变化好像没有啊。。。只有左右晃动才有波形。。。请问楼主这是何原因

Jyoun 发表于 2015-8-20 09:00:39

好贴，感谢楼主，收藏了。

fighter_ck 发表于 2015-8-28 17:30:10

怎么我用console 老是编译出错了

fighter_ck 发表于 2015-9-25 16:03:12

你好，用了你的固件，顶不住啊。ps：我没用超声波，只用了一个光流模块。

fighter_ck 发表于 2015-9-25 18:14:01

勤奋的MCU 发表于 2015-8-3 20:32
你好，请问你的光流能实现悬停了么？效果怎么样呢？

你好，请问你现在实现光流定点了吗?有没有结合超声波使用？

光轮发表于 2015-10-28 14:41:10

好东西，mark了！

ly8008csko 发表于 2015-10-28 14:48:12

支持一个，收藏了

EngineeringBoy 发表于 2015-10-28 18:53:27

yuwangwangyu 发表于 2015-6-12 21:55
我已经移植到MDK下了

能把你的工程分享下吗？谢谢啦{:hug:}

RudeBoy 发表于 2015-10-28 19:15:59

顶一个，楼主研究的很认真，看似花了不少时间和心思

RudeBoy 发表于 2015-10-28 19:20:46

golaced 发表于 2015-6-15 11:06
通过光流测试波形可以看到，光流传感器非常容易受外界干扰，传感器上下移动，倾斜和左右旋转都会造成读数大 ...

这个数据本身就容易收到干扰，确实需要外围的传感器数据用来纠正，不过相对就复杂了很多

RudeBoy 发表于 2015-10-28 19:22:54

yuwangwangyu 发表于 2015-6-12 21:55
我已经移植到MDK下了

漂亮，对pix家的那种指令行的make方式确实反感，移植都需要哪些注意的呢，复杂程度如何，一直想移植，但是代码比较大，想找一个相对小一点的工程移植试试

759648442@qq.co 发表于 2015-10-29 13:19:52

问问十几块的那种能装到pix上用吗

yuwangwangyu 发表于 2015-10-29 22:06:19

意义不大用GPS就可以定的很稳

golaced 发表于 2015-11-10 11:14:47

更新光流定位测试视频，死区有点大定位调整不足
http://pan.baidu.com/s/1ntIS4fn

golaced 发表于 2015-11-11 10:13:39

光棍节更新光流定点视频 http://pan.baidu.com/s/15bO2E

hemeizhi 发表于 2015-11-14 10:31:33

光流悬停的飞控用的啥？apm吗

golaced 发表于 2015-11-15 12:45:11

hemeizhi 发表于 2015-11-14 10:31
光流悬停的飞控用的啥？apm吗

自己的飞控 OLD-X

molys 发表于 2015-11-20 11:03:37

golaced 发表于 2015-11-11 10:13
光棍节更新光流定点视频 http://pan.baidu.com/s/15bO2E

楼主，可以发最新的Flow固件测试吗？飞控代码flow_postion_estimat.c有修改吗？我室内测试过你的 “px4flow.zip (43.96 KB, 下载次数: 110) ”，在Loiter模式与STABILIZE模式差不多，定位效果不是很好，比室外GPS模式的（Loiter）模式差。

golaced 发表于 2015-11-20 11:59:14

molys 发表于 2015-11-20 11:03
楼主，可以发最新的Flow固件测试吗？飞控代码flow_postion_estimat.c有修改吗？我室内测试过你的 “px4fl ...

你试试这个行不行

molys 发表于 2015-11-20 13:33:58

golaced 发表于 2015-11-20 11:59
你试试这个行不行

谢谢楼主！我先测试过，体验体验，再与你分享！

molys 发表于 2015-11-20 14:27:03

楼主，比以前改善了很多的，可以共享一下源代码？

molys 发表于 2015-11-20 22:56:20

golaced 发表于 2015-11-20 11:59
你试试这个行不行

楼主，新的固件，室内定位效果不错，请教楼主你在Flow-master 代码修改了哪些东西，可以详细说说吗？正在做课题报告，过几天上交；请楼主帮帮忙，我的邮箱1841523243@qq.com

golaced 发表于 2015-11-21 15:48:08

molys 发表于 2015-11-20 22:56
楼主，新的固件，室内定位效果不错，请教楼主你在Flow-master 代码修改了哪些东西，可以详细说说吗？正在 ...

我也是在网上下的你可以看看国外的论坛有没有人改过

molys 发表于 2015-11-22 09:46:22

golaced 发表于 2015-11-21 15:48
我也是在网上下的你可以看看国外的论坛有没有人改过

无人机大神，请教大神国外哪一个网站，麻烦指点迷津!

molys 发表于 2015-11-23 16:32:09

golaced 发表于 2015-11-21 15:48
我也是在网上下的你可以看看国外的论坛有没有人改过

楼主，新的固件，室内定位Loiter模式还是会漂，Loiter模式没有GPGS稳定。

golaced 发表于 2015-11-23 20:14:22

molys 发表于 2015-11-23 16:32
楼主，新的固件，室内定位Loiter模式还是会漂，Loiter模式没有GPGS稳定。

zzzzzzzzzzzzzzzzzzzzzzzzzzz

molys 发表于 2015-11-24 09:14:50

golaced 发表于 2015-11-23 20:14
zzzzzzzzzzzzzzzzzzzzzzzzzzz

楼主，你好！你的这个固件将Flow-master的I2C关掉了吗？怎么I2C无数据出来呢？

molys 发表于 2015-11-24 09:52:20

golaced 发表于 2015-11-23 20:14
zzzzzzzzzzzzzzzzzzzzzzzzzzz

楼主，麻烦你不要关掉I2C通信，我的飞控板与flow-master是I2C通信的，飞控板测不了光流数据

molys 发表于 2015-11-24 15:20:13

本帖最后由 molys 于 2015-11-24 16:17 编辑

golaced 发表于 2015-11-23 20:14
zzzzzzzzzzzzzzzzzzzzzzzzzzz

楼主，在QGC上看图像质量，固件px4flow_hao1比固件pxflow_hao12好一些，但固件px4flow_hao1没有I2C通信，PIXHAWK 飞控光流通信是I2C，无法测试到实际结果，请不要屏蔽I2C.谢谢楼主，谢谢大神！

golaced 发表于 2015-11-24 21:42:26

molys 发表于 2015-11-24 16:41
楼主，是加了如下文件吗：
#include
#include

同学抱歉我也不怎么清楚光流的程序那个hao1是飞行试验室的但是不用他们的光流硬件xy读数只是反了下向你可以试试加上滤波会不会好一点，欢迎交流飞控其他方面的知识

页: [1] 2

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

PX4 FLOW光流 应用关键问题讲解

PX4 FLOW光流应用关键问题讲解