（源码解读）position_estimator_inav_main解读（如何启动光流）

阅读工具：source insight
技术交流：[email protected]
源码版本：Firmware（原生固件）

如有需求可以索要word版

这篇博客彻底解决了我无法切换模式的问题，但是还存在几个问题

1. 目前还是没有看到定点效果，有可能是磁场干扰（我实验室有几个磁悬浮装置。。。）

也有可能是需要启动LPE

1. 没有发现光流与GPS数据融合，据说光流在此函数中是辅助GPS的，可有可无，但是我看程序的光流和GSP数据是独立提供的（希望大神指点）

分析问题的思路（我一直认为思路很重要）

目的：我希望实现光流定点效果

问题：无法切换posctl飞行状态（可能是程序问题，也可能是光流数据问题）

思路：位置估计函数最终一组数据给位置控制，已知GPS可以实现定点,可以认为位置控制部分是对的，也就是说控制算法不用管只需要检查光流部分是否正确。 那么我只需要找到哪些数据可以由光流传感器提供，并检查其数值是否正确。

这是位置估计最后产生的值，我们倒过来往前找，can_estimate_xy

bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy || use_mocap;可以看到can_estimate_xy是与use_flow光流有关的
接下来两个与GPS有关跳过，看x_est, y_est，z_set

Lidar与flow提供了accel_bias_corr[3]数组的3个数

accel_bias_corr[3]数组的3个数传递给了acc_bias[3];

acc_bias[3]最终传递给 acc[3];

这写着惯性预测滤波，第一次给定了x_set[2], y_set[2], z_set[2]数组的值，Z轴的值是lidar提供，光流只提供水平值

预测结束，马上就进行了第二次滤波，当输入是0时z_set[0],z_set[1]都被赋值，lidar提供这两个数据

当输入时1时，这个函数只给x_est[1], y_est[1]赋值，所以光流第2次滤波只改修改了这两个值。
综上所述，光流加上雷达提供了x_est[2], y_est[2], z_est[2],所有数据，但是为什么有这么多滤波，暂时不知道。
att.yaw,其实就是vehicle_attitude_s.yaw

通过订阅来的，那么是谁发布的呢？
Attitude_estimator_q_main.cpp (firmware\src\modules\attitude_estimator_q):
orb_publish_auto
(ORB_ID(vehicle_attitude), &_att_pub, &att, &att_inst,ORB_PRIO_HIGH);
姿态估计发布的（找topic的发布者好难，有谁有简单的方法教我一下吧）
dist_bottom_valid， lidar提供

eph(水平定位精度),epv（垂直定位精度）常数

到这所有数据就分析完了 总结一下，光流能提供所有水平有关的值，lidar提供垂直方向的值，为什么要分析lidar？

看见没 想用光流 第一个条件就是lidar是否有效，在位置估计里雷达就是距离传感器，对应的实际就是超声波传感器，（实测lidar的数据都是超声波提供的）

所以启用光流的核心问题就是如何启动lidar_valid



可以看出lidar_valid与3个数据有关params.enable_lidar_alt_est，
lidar.current_distance > lidar.min_distance，lidar.current_distance < lidar.max_distance
其实我们已知光流是有读数，那么后两个判读肯定是能成立的（当前高度大于30cm,小于5m）
主要就是params.enable_lidar_alt_est（为了保险我在程序里加上标志位进行了判读，确实就是第一个条件一直不成立）

找到这个赋值函数，注意这个函数是把h的值给P，（不是把p给h）!!!!!!


在里把这个0改成1就启动了
在函数里找到了，想到这种设置应该在地面站也有，于是去QGC找果然

在这里改应该也是一样的，亲测现在可以自由切换到Posctl（定点模式了）虽然还是不能实现定点功能，但是解决了第一个问题还是很开心。