四轴飞控DIY Mark4 - RTH/GPS Rescure

Posted 2022-10-07 lida2003

四轴飞控DIY Mark4 - RTH/GPS Rescure

• 1. RTH/GPS Rescure功能介绍
• • 1.1 目前(2022.10.01)BetaFlight在该功能点上的规划
  • 1.2 目前(2022.10.01)iNavFlight在该功能点上特性和要求
• 2. RTH/GPS Rescure验证准备
• • 2.1 硬件选择
  • 2.2 注意事项
  • • 2.2.1 安装在Gopro支架上
    • 2.2.2 安装在电池上方
    • 2.2.3 安装在机架上方
    • 2.2.4 安装在尾部GPS支架上
    • 2.2.5 不要安装在电源线上
  • 2.3 安装位置
  • 2.4 软件配置
  • • 2.4.1 地面配置软件和固件版本
    • 2.4.2 系统配置使能磁力计
    • 2.4.3 磁力计传感硬件方向对齐
    • • 2.4.3.1 方向问题
      • 2.4.3.2 解决方法
      • 2.4.3.3 设置方向
• 3. RTH/GPS Rescure测试
• • 3.1 RTH/GPS Rescure(Sensor Function)
  • • 3.1.1 城北公园-阴天(濛濛细雨前)
    • 3.1.2 梅林南路-阴天
  • 3.2 RTH/GPS Rescure(Mannual Trigger)
  • 3.3 RTH/GPS Rescure(50mW Signal Lost)
• 4. RTH/GPS Rescure远距离实测
• 5. 总结
• 6. 参考资料

穿越机的玩法很多，大体上就是以下几个场景：

1. 竞赛(racing)
2. 花飞(freestyle)
3. 航拍

总的来说，对于我们一般人来说，组装了就要去好风景的地方从天空端角度去感受下不一样的体验和感受，所以那就不可能一直在熟悉的地区飞。

在不熟悉的地方飞，就需要RTH/GPS Rescure功能来避免信号丢失而失控丢机，将美好的心情带到谷底。

下面我们主要从BetaFlight固件的角度来验证和讨论下这个功能点。

1. RTH/GPS Rescure功能介绍

BetaFlight固件这个功能叫GPS Rescure，而在iNavFlight固件这个功能叫RTH, Return To Home。

鉴于穿越机自身系统复杂度，硬件算力，以及有限传感器等原因，我认为GPS Rescure的说法相对来说更加贴切。其目的就是用一个最简单的方法，将飞机到达可控的位置，然后由人来接管，进而确保返航。而Return To Home这个概念容易让人联想到是安全返回的方法，实际上这并不一定安全，取决于返航路径上的实际情况(只能说大部分情况可以)。所以这里还是建议，不管使用哪种固件，返回过程需要全程监控，适时切换人工控制，从而保证飞行安全。

注1：相对与BetaFlight的GPS Rescue来说iNav的RTH功能，更加适合期望能够自动降落的小伙伴。
注2：RTH自动返航降落功能涉及很多问题：比如：返航路径上障碍物(静态、动态)，障碍物高度和返航高度，障碍物体积与返航路线等诸多问题，这里不做展开。
注3：关于真正的RTH，可能需要伴飞电脑，通过路径规划和更多传感系统来避障以及空地配合来完成，这个倒是可以考虑ROS系统上进行探讨。

1.1 目前(2022.10.01)BetaFlight在该功能点上的规划

• GPS Rescue Mode for 4.1 to 4.3

The only purpose is to bring the quad back into range so you can retake control as soon as possible.

• GPS Rescue for 4.4

Brings improvements in the altitude, velocity, flightpath accuracy and landing behaviour of the GPS Rescue code, and includes many error handling changes.

1.2 目前(2022.10.01)iNavFlight在该功能点上特性和要求

• Requires accelerometer, compass and GPS sensors.
• If barometer is present, RTH will maintain altitude during the return. When home is reached, a copter will attempt automated landing.
• If barometer is NOT present, RTH will fly directly to home, altitude control here is up to pilot.
• RTH has 6 different modes of operation

0 (NAV_RTH_NO_ALT) - keep current altitude during whole RTH sequence (nav_rth_altitude is ignored)
1 (NAV_RTH_EXTRA_ALT) - climb to current altitude plus extra margin prior to heading home (nav_rth_altitude defines the extra altitude (cm))
2 (NAV_RTH_CONST_ALT) - climb/descend to predefined altitude before heading home (nav_rth_altitude defined altitude above launch point (cm))
3 (NAV_RTH_MAX_ALT) - track maximum altitude of the whole flight, climb to that altitude prior to the return (nav_rth_altitude is ignored)
4 (NAV_RTH_AT_LEAST_ALT) - same as 2 (NAV_RTH_CONST_ALT), but only climb, do not descend
5 (NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT) - Same as 4 (NAV_RTH_AT_LEAST_ALT). But, if above the RTH Altitude, the aircraft will gradually descend to the RTH Altitude. The target is to reach the RTH Altitude as it arrives at the home point. This is to save energy during the RTH.

2. RTH/GPS Rescure验证准备

BetaFlight GPS Rescure功能需要的硬件传感功能：

1. 【MUST】Acc
2. 【MUST】Gyro
3. 【MUST】GPS
4. 【OPTION】Baro
5. 【OPTION】Compass

BetaFlight, GPS-Rescue-Mode-for-4.1-to-4.3指出M8N和BN880是两块验证过的带Compass的GPS模块。

另外，心理上也要做好准备，GPS Rescure不是 RTH，而是信号丢失后的救机。

2.1 硬件选择

本次测试用机AOCODA F405自带以下硬件传感器：

1. Acc
2. Gyro
3. Baro

配件M8N GPS模块，自带以下硬件传感器：

1. GPS(采用ublox协议)
2. Compass

注：该GPS的搜星能力是比较好的，以前在F450上使用时，通常在13-15颗星的样子。

考虑到兼容市场上BN880 GPS模块(含Compass)，我们将M8N GPS模块的线缆按照BN880模块的线缆顺序以及SH1.25连接件进行对接，方便后续替换测试。

2.2 注意事项

鉴于GPS和Compass的特殊性，其安装位置需要特别注意：

1. GPS陶瓷天线朝上，且天线周边不要有遮挡物，方便搜星
2. Compass的工作原理是通过磁场来感知方向，因此周边要减少电磁干扰

通常建议GPS & Compass通过支架部署安装到飞机的最高点，远离电子器件干扰，同时确保周边开阔易于接受GPS卫星信号。常规5寸及以下小机型安装大体位置有以下几种：

2.2.1 安装在Gopro支架上

2.2.2 安装在电池上方

2.2.3 安装在机架上方

2.2.4 安装在尾部GPS支架上

2.2.5 不要安装在电源线上

2.3 安装位置

鉴于测试用机整体情况：

1. 没有尾部GPS支架(计划用M8N来测试，也没有结构的打印机，不具备自己做的前提条件)
2. GoPro支架(尚未打算装狗，只有底座，没有狗笼~~)
3. 头部摄像头后方位置没有电子器件（主要是摄像头和GPS接线和插头），并且远离电机电源线
4. 当前3S电池唯一的问题是靠近GPS陶瓷天线，一侧有部分高度遮挡(应该问题不大，具体实测吧)

基本上满足：干扰少，周边开阔

2.4 软件配置

2.4.1 地面配置软件和固件版本

# status
MCU F40X Clock=192MHz (PLLP-HSE), Vref=3.29V, Core temp=39degC
Stack size: 2048, Stack address: 0x1000fff0
Configuration: CONFIGURED, size: 4100, max available: 16384
Devices detected: SPI:1, I2C:1
Gyros detected: gyro 1 locked
GYRO=MPU6000, ACC=MPU6000, BARO=BMP280, MAG=HMC5883
OSD: MAX7456
System Uptime: 14 seconds, Current Time: 2022-10-05T10:13:56.632+00:00
CPU:62%, cycle time: 128, GYRO rate: 7812, RX rate: 15, System rate: 9
Voltage: 1101 * 0.01V (3S battery - OK)
I2C Errors: 0
SD card: Not configured
Arming disable flags: RXLOSS CLI MSP

2.4.2 系统配置使能磁力计

2.4.3 磁力计传感硬件方向对齐

根据硬件规格书设置磁力计传感器的方向。

2.4.3.1 方向问题

1. 这里提及硬件规格书是正确的，但是往往很多硬件规格书不详细。
2. 尤其随着技术的发展，传感器物理特性和软件相关，这就是嵌入式的重要性。
3. 磁力计传感器主要目的是为了检测地球磁场，从而要避免周边电磁干扰影响采集到的数据，进而错判了地球磁场方向。
4. 更可悲的是这些硬件规格书通常没有详细到传感器物理特性规格（芯片是有的）。

2.4.3.2 解决方法

1. 根据规格书磁力计物理安装方向设置，比如： CW0/CW90/CW180/CW270/CW0flip/CW90flip/CW180flip/CW270flip
2. 找到PCBA上的磁力计芯片，根据芯片规格书找到方向

为什么上面说了这么一段话，其主要目的就是指出嵌入式不仅仅是硬件，软件，还有更多的物理含义在里面，需要从系统的角度考虑问题，进而细化到规格参数。检测磁场方向和芯片物理安装焊接的方向有关，因此M8N模块需要提供磁力计传感器芯片默认的方向（通常我们默认为CW0），如果不一致需要在规格书中给出，尤其是那种有外壳看到内部芯片的模块，笔者的这款就是这种情况。

笔者的这款已经装上了，所以没有PCBA的拍照，不过找了一个类似的示意(HMC5883L的安装方式和M8N一致)

注1：这里芯片左上角有一个点，这个点在芯片上也可以看到，其主要目的是标识顺时针第一脚用的，这里可以提供芯片xy轴指向方向的参考点。
注2：关于M8N规格书：这里找了一份类似的但不是笔者的这款，可以参考Holybro_M8N_GPS_Quick_Start_Guide ，毕竟holybro这个牌子还是可以的。

2.4.3.3 设置方向

成熟的开源社区有大量的信息，只要用社区认可的硬件的话，而BetaFlight和iNav都是从cleanflight克隆出来的，一个注重稳定，一个注重性能。根据iNav, GPS–and-Compass-setup，可以了解到M8N模块在箭头对准机头前方的时候，设置CW270Flip：

而笔者在实际绑扎的方便性，实际方向与原有箭头呈135度角，因此实际应该设置CW135Flip。

综上所述：

1. 设置自定义角度设置
2. 命令行脚本设置 align_mag / mag_align_pitch / mag_align_yaw

# set align_mag = CUSTOM
# set mag_align_roll = 0
# set mag_align_pitch = 1800
# set mag_align_yaw = 1350
# save
…
# get mag
align_mag = CUSTOM
Allowed values: DEFAULT, CW0, CW90, CW180, CW270, CW0FLIP, CW90FLIP, CW180FLIP, CW270FLIP, CUSTOM
Default value: DEFAULT

mag_align_roll = 0
Allowed range: -3600 - 3600

mag_align_pitch = 1800
Allowed range: -3600 - 3600
Default value: 0

mag_align_yaw = 1350
Allowed range: -3600 - 3600
Default value: 0

3. RTH/GPS Rescure测试

3.1 RTH/GPS Rescure(Sensor Function)

3.1.1 城北公园-阴天(濛濛细雨前)

测试视频：BetaFlight Mark4 + GPS&Compass (FunctionTest)

• GPS搜星能力一般

6颗星耗时: > 1分33秒
7颗星耗时: > 6分36秒
8颗星耗时: > 7分11秒

• Compass东南西北指向

东南西北及转向正确

• Compass Home指向

基本正确，略偏右

3.1.2 梅林南路-阴天

测试视频：BetaFlight Mark4 + GPS&Compass + Meilin South Road(FunctionTest)

• GPS搜星能力一般

06颗星耗时: > 6分27秒
07颗星耗时: > 6分35秒
08颗星耗时: > 7分15秒
09颗星耗时: > 8分07秒
10颗星耗时: > 8分35秒
11颗星耗时: > 8分45秒

• Compass东南西北指向

东南西北及转向正确

• Compass Home指向

基本正确，略偏右

3.2 RTH/GPS Rescure(Mannual Trigger)

测试视频：BetaFlight Mark4 + GPS Rescure(Mannual Trigger) + Meilin South Road

06分55秒：最远距离740米，RC信号-87dBm
07分08秒：手动触发GPS Rescure，飞机返航
07分50秒：手动解除GPS Rescure，手控下降
11分15秒：查看骑行车队，发现无人机一台

注1：本视频，设置遥控发射功率50mW，而实际在700米处，仍未遥控信号丢失。
注2：后半段小插曲，发现一架“敌机”，经确认是在航拍。

• 发现高度计出现异常，出现甚至-10米的情况，经确认是GPS的高度存在严重误差。

调整原默认设置
position_alt_gps_min_sats = 10
position_alt_baro_fallback_sats = 7
到新设置来尽量确保卫星少的时候高度大误差问题
position_alt_gps_min_sats = 15
position_alt_baro_fallback_sats = 13

3.3 RTH/GPS Rescure(50mW Signal Lost)

鉴于50mW 700米信号未丢失，后续考虑关机或者飞的更远测试。

4. RTH/GPS Rescure远距离实测

测试终止场景：

1. 图传信号丢失(5.8GHz 600mW)
2. 遥控信号丢失(915MHz 50mW Dynamic On)

5. 总结

TBD，尚未全部完成，先发布，关注的朋友可以常来看看进展

6. 参考资料

【1】BetaFlight, GPS Rescure
【2】iNav, Return To Home
【3】BetaFlight, GPS-Rescue-Mode-for-4.1-to-4.3
【4】BetaFlight, GPS-Rescue-for-4.4
【5】iNav, GPS–and-Compass-setup
【6】