ArduPilot开源代码之MatekSys Optical Flow 3901-L0X

1. 源由

之前介绍过MatekSys Optical Flow 3901-L0X模块：

【1】传感模块：MATEKSYS Optical Flow & LIDAR 3901-L0X
【2】iNavFlight之MSP v2 Sensor报文格式

当时主要是在inav固件上进行测试，整体感觉效果一般。

鉴于本次打算将电传模块换到VTOL飞机上去飞固定翼，
【1】Ardupilot开源飞控之VTOL之旅：配件规格
【2】Ardupilot开源飞控之VTOL之旅：配件试装

所以借着这个机会，将MatekSys Optical Flow 3901-L0X替换Mark4的电传，看下Ardupilot效果如何？

2. 安装

1. 电源供电5V
2. UART4_Tx/UART4_Rx用于通信
3. 安装方法参考：传感模块：MATEKSYS Optical Flow & LIDAR 3901-L0X

3. 参数配置

3.1 配置光流定位

FLOW_TYPE = 7 (MSP)
FLOW_FXSCALER = -800
FLOW_FYSCALER = -800
SERIAL4_PROTOCOL = 32 (MSP)
SERIAL4_BAUD = 115 (115200 bps)

3.2 配置激光测距

RNGFND1_TYPE = 32 (MSP)
RNGFND1_MIN_CM = 8 to set range finder’s minimum range to 0.08m
RNGFND1_MAX_CM = 200 to set range finder’s maximum range to 2m
RNGFND1_ORIENT = 25 (Facing Down)

注：文档中不推荐使用这颗板载的激光测距传感，可能是测量距离太近(0~2m)，或者数据的稳定性受到环境影响较大。

3.3 辅助配置

1. Loiter和PosHold飞行模式下，需要提供：光流+测距+GPS定位信息。

2. FlowHold飞行模式下，仅需提供：光流位置信息。

3. 定位优化：(GPS/IMU/OpticalFlow在15cm距离以内，通常对性能没有影响)；距离重心的位置；

Optical Flow

FLOW_POS_X, FLOW_POS_Y, FLOW_POS_Z
FLOW_POS_X = 0.05 set to 5 cm forward
FLOW_POS_Y = 0
FLOW_POS_Z = 0

IMU

INS_POS1_X, INS_POS1_Y, INS_POS1_Z
INS_POS2_X, INS_POS2_Y, INS_POS2_Z
INS_POS3_X, INS_POS3_Y, INS_POS3_Z

GPS

GPS_POS1_X, GPS_POS1_Y, GPS_POS1_Z
GPS_POS2_X, GPS_POS2_Y, GPS_POS2_Z

Range Finder (Sonar or Lidar)

RNGFND1_POS_X, RNGFND1_POS_Y, RNGFND1_POS_Z
RNGFND2_POS_X, RNGFND2_POS_Y, RNGFND2_POS_Z

Visual Odometry

VISO_POS_X, VISO_POS_Y, VISO_POS_Z

4. 测试

4.1 光流数据测试

1. opt_m_x, opt_m_y, opt_qua显示非零值，持续跳动。

2. OF.flowX, OF.bodyX, IMU.GyrX 趋势和大小基本一致

3. OF.flowY, OF.bodyY, IMU.GyrY 趋势和大小基本一致

4.2 测距数据测试

状态栏 rangefinder1不为零，且随着姿态、高度变化，数值缓慢线性变化。

4.3 飞行注意事项

1. 飞行预检查，取消GPS检查项。

注：室内无GPS，若不取消，无法arm。

2. 第一次短距离STABILIZE 飞行（通常现在都是4.4.0以上的固件，所以大部分情况下采用的是EKF3算法）

EK3_SRC1_VELXY = 0 //we don’t want the optical flow being used by the EKF at this stage

短距离飞行后，下载日志检查：

• OF.bodyX，OF.bodyY 与IMU.GyrX, IMU.GyrY 都有变化且趋势一致
• EKF5.meaRng变化趋势和范围与实际飞机高度变化一致

注：检查日志数据EKF5.meaRng，最新4.4.0固件XKF5.rng。

• HereFlow calibration - what and where is EKF5.meaRng
• What happened to meaRng?

3. 第二次飞行

室内FlowHold起飞

EK3_SRC1_VELXY = 5 //OpticalFlow
EK3_SRC1_POSXY = 0 //不要GPS

室外Loiter起飞

EK3_SRC1_VELXY = 5 //OpticalFlow
EK3_SRC1_POSXY = 3 //GPS

注：通常户外是有GPS的，所以Loiter 是可以正常起飞；但是室内没有GPS，此时就需要使用FlowHold。

参考：Prearm: Need Position Estimate

4.4 实际飞行测试

这里配置了第二次室内飞行参数，验证MatekSys Optical Flow 3901-L0X的使用效果。

Ardupilot 4.4.0 Mark4 + MatekSys Optical Flow 3901-L0X + FlowHold

对比iNav光流悬停，详见：传感模块：MATEKSYS Optical Flow & LIDAR 3901-L0X

iNav

5. 参考资料

【1】传感模块：MATEKSYS Optical Flow & LIDAR 3901-L0X
【2】iNavFlight之MSP v2 Sensor报文格式
【3】MatekSys Optical Flow 3901-L0X