四轴飞控DIY Mark4 - RTH/GPS Rescure

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

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测试

杭州梅林南路这片区域是不错的。

1. 飞机掉茶地，炸机问题也不大，容易捡回来
2. 没有什么人，安全性也好
3. 地方也相对开阔，手动也可以练习
4. 周边风景优美
5. 车子桥下可以免费停

BetaFlight Mark4 + 梅灵南路 + 无牙崽2晴天

注：但是要注意，不要飞的太高，建议200米以下，桥下到山顶的高度大概是150米。因为是航路，所以不要穿云，影响飞行安全，谢谢！！！

3.1 RTH/GPS Rescure(Sensor Function)

3.1.1 【测试通过】城北公园-阴天(濛濛细雨前)

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

BetaFlight Mark4 + GPS&Compass (功能测试)

• GPS搜星能力一般

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

• Compass东南西北指向

东南西北及转向正确

• Compass Home指向

基本正确，略偏右

3.1.2 【测试通过】梅林南路-阴天

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

BetaFlight Mark4 + GPS&Compass + 梅灵南路(功能测试)

• 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

BetaFlight Mark4 + GPS Rescure(Mannual Trigger) + 梅灵南路

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米信号未丢失，后续考虑遥控关机进行测试，以确保不会丢机或者其他安全事故。

3.3.1 【测试失败】第一次测试，飞机坠落茶叶地

测试视频：BetaFlight Mark4 + GPS Rescure + Signal Lost + Fail

BetaFlight Mark4 + GPS Rescure + Signal Lost + Fail

0分42秒， 手动触发GPS救援 ==》成功
3分01秒， 遥控关机触发GPS救援 ==》失败

经过各种搜索，发现信号丢失测试GPS有一个遥控丢失后，按键默认值的问题。

3.3.2 【测试通过】第二次测试，信号丢失返航；操作失误，飞机坠落茶叶地

测试视频：BetaFlight Mark4 + GPS Rescure + Signal Lost + SOS

BetaFlight Mark4 + GPS Rescure + Signal Lost + SOS

2分06秒， 遥控关机触发GPS救援 ==》成功
2分24秒, RXLOSS 调整为 FAILSAFE，提示已经连上飞机
2分25秒（大概这段时间）， 遥控开机恢复默认按键(操作失误)
2分31秒，FAILSAFE 调整为遥控设置，disarm导致飞机掉下来了

仔细回顾每个步骤和视频，最后确认是飞机返航回来的时候：

1. 有点手忙脚乱
2. 通常，遥控器默认开机都要恢复默认按键位置的
3. 自己没有经验，一直以为没有连上
4. 其实系统已经显示连上了，并且在提示FAILSAFE（恢复到默认返航，arm飞机，按键拨动到resucre位置）

结果没有经验+手忙脚乱，导致坠落。（飞机依然掉到茶叶上，只是915天线有根塑料管折断，其他都完好无损。不幸中的大幸！）

3.3.3 总结

这里针对GPS救援功能，做了相关重要配置项和远航前准备工作。

3.3.3.1 重要配置项

1. Modes页面主要是配置【遥控按键触发GPS救援】
2. FailSafe页面配置三大块
   • 通道无遥控时默认设置值 （不设置的时候，通常是低值）
   • Stage2阶段采用GPS Rescure方式
   • GPS Rescure方式参数设置（后面重要的参数单独详细解释，至于相对不重要的，请大家自行学习）
3. Angle 35 主要指倾斜角度最大不超过35度，当大风的情况，35度倾斜还飞不回来，那就惨了，请根据实际情况调整。
4. Initial Altitude (meters) 100 最低返航初始高度100米

a) 低空穿越树林，洞穴的朋友，自行注意了(信号丢失直接上天，卡树上得不偿失，还不如直接掉落呢。
b) 一般山区有很多山头，高度都不一样，翻过山头信号丢失，结果山头比设置的返航高度高，那就没辙了
c) 我后来仔细看了下，发现最高只能100米，所以山区的时候，自己的站位（起飞点）一定要高啊（信号也好点）
# get gps_rescue_initial_alt
gps_rescue_initial_alt = 100
Allowed range: 20 - 100
Default value: 50

5. Minimum distance to home (meters) 150 最小GPS救援距离，小于这个距离GPS救援无效。所以测试的时候请大家飞300米
6. Minimum satellites 6 至少需要6颗，默认8颗，请大家不能设置小于6颗，谢谢！
7. Altitude Mode （Fixed Altitude）是指固定高度返航。否则接近起飞点的时候会下降，这个时候是个三角形下降过程，需要配置Desend distance （meters），默认200米，需要注意这个三角并不那么三角，要数学好，地形熟悉的朋友可以考虑使用，一般的朋友，还是固定高度返航吧。

注：这里有个Allow arming without fix是指没有GPS也可以解锁飞，但是要注意没有GPS是没有救援功能的。切记，切记，切记！！！
注：这个模式页面是没有接遥控器的，为什么会卡在2000这个档位，就是因为Failsafe页面设置了AUX6 = 2000。否则信号一丢，飞机不会回来，只会丢机，一定要注意！

3.3.3.2 远航前准备

1. 确认测试区域最低高度，并设置救援返航高度大于该最低高度（要有容错）
2. 确认测试救援点下方安全（不仅飞机掉落安全，人员财产安全）
3. 确认飞机所有重要配置项正确
4. 测试项目1：【50米环绕飞行】确保指南针指向基本正确。全程跟踪返航指向起飞点。
5. 测试项目2：目视低空飞行300米，确保返航路径安全。测试点【按键触发】GPS救援功能
6. 测试项目3：目视低空飞行300米，确保返航路径安全。测试点【遥控关控，信号丢失触发】GPS救援功能

注1：全程眼镜目视跟踪飞机，测试功能成功后，遥控手动接管飞机，确保安全和受控降落。
注2：要注意遥控器关机之后，开机后的各项操作，不要像我手忙脚乱的。

4. RTH/GPS Rescure远距离实测

4.1 测试条件

测试视频：发射机50mW功率 和/或 最大发射机功率1000mW

测试终止场景：

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

4.2 测试分析

【1】【图传信号丢失】BetaFlight Mark4 + Meilin South Road + 无牙崽2阴天

BetaFlight Mark4 + 梅灵南路 + 无牙崽2阴天

分析：可能是山头将图传5.8G的信号挡住了，所以导致信号差。915MHz直接切了GPS救援模式。

【2】【Rx信号丢失】BetaFlight Mark4 + 十里琅珰 + 50mW测距

分析：
1） 据高频头硬件厂家反馈：应该轻松5km，如果低于5km，不是遮挡就是当地环境存在较多电磁干扰。
2） 实际飞了2km，可能是由于周边干扰。山头有一个基站天线立着（50m左右）。

【3】【Rx信号丢失】BetaFlight Mark4 + 十里琅珰 + ELRS3.0 + 500mW_Dyn

BetaFlight Mark4 + 十里琅珰 + ELRS3.0 + 500mW

分析：
1）之前ELRS 2.5.1；用了最新的ELRS3.0，设置500mW + Dynamic，期望能飞到5km。
2）实际2.5km飞行，触发GPS救援。
3）可能由于3.0固件采用了SNR判断信号质量，经常出现警告（链路SNR告警非常正确）
4）据发射机厂家反馈：建议使用固定功率

推荐
1）使用3.0的ELRS固件
2）唯一的问题：远航更远可能这个告警没什么用，因为这个信号来源于接收机回传数据，而GPS救援触发是飞控判断发射机信号丢失。
3）固件更新请参考：【7】

– 后续会不断测试，并补充的，对远航距离比较关注，或者期望能尽快更新的，可留言！–

5. 总结

很多朋友都反馈说BetaFlight的GPS Resucre功能容易丢机。这个可能与BetaFlight开源固件的目标(racing drones and amazing flight dynamics )有关。iNav开源固件的目标才是稳定飞行。

不过最近BetaFlight也在加强这方面的功能，详见：BetaFlight, GPS-Rescue-for-4.4。

总的来说，确实没有一份比较完整的设置GPS Rescure的文档，能面面俱到的讲述如何进行配置，希望我趟过的坑，对大家有点帮助！！！

注：有帮助点赞，有不足，请评论伺候，万分感谢！！！

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】BetaFlight, 4.3-Tuning-Notes
【7】TX12 + ExpressLRS 915MHz RC控制链路配置及问题汇总