BetaFlight代码解析（9）— GPS定位系统

Betaflight 中的 GPS 定位系统处理来自各个模块的 GPS 数据，并将其与其他传感器的数据相结合，以提供精确的位置估计和高度计算。该系统是导航功能、返航功能和高度控制的基础。

系统架构

GPS定位系统由GPS数据处理、传感器融合和位置估计组件组成，这些组件协同工作以提供精确的位置和高度信息。

GPS数据处理流程

核心数据结构

GPS状态机

GPS 系统通过状态机运行，该状态机负责初始化、配置和数据处理。

GPS初始化和配置

gpsInit()GPS系统由以下函数初始化：

1. 设置数据结构和默认值
2. 找到已配置的 GPS 串口
3. 使用适当的设置打开串口
4. 开始波特率检测过程

对于 MSP 和虚拟 GPS 提供商，无需进行串行配置，系统直接跳到初始化状态。

配置过程

根据GPS模块类型的不同，系统遵循不同的配置步骤：

UBLOX 配置

UBLOX 模块通过一系列 UBX 协议消息进行配置：

该过程的状态通过ubloxStatePosition_e定义每个配置步骤进行跟踪。

NMEA 配置

对于基于NMEA的GPS模块：

1. 系统设置波特率
2. 发送任何已配置的自定义命令gpsConfig()->nmeaCustomCommands

GPS数据结构

GPS系统使用以下几种关键数据结构：

GPS解决方案数据

gpsSolutionData_t包含已处理后的GPS数据，其中包括：

• 位置（纬度、经度、海拔）
• 速度（地面速度、三维速度）
• 航向/标题
• 精度值的稀释
• 准确度估计
• 卫星数量

GPS状态数据

gpsData_t跟踪 GPS 系统的运行状态：

• 状态机的当前状态
• 统计数据（错误、超时）
• 配置状态
• 模块信息

卫星信息

GPS_svinfo_t提供每颗卫星的详细信息：

• 频道号
• 卫星识别
• 信号质量和强度

UBLOX协议实现

Betaflight 实现了对 UBLOX 协议的全面支持，并可在多个模块世代中进行版本特定的配置处理。

模块版本检测和配置

UBLOX 配置状态机

UBLOX 配置遵循以下定义的详细状态序列ubloxStatePosition_e：

状态	描述	功能
UBLOX_DETECT_UNIT	检测模块版本	ubloxSendPollMessage(MSG_MON_VER)
UBLOX_SLOW_NAV_RATE	设置慢速导航	ubloxSetNavRate()
UBLOX_MSG_DISABLE_NMEA	禁用 NMEA 输出	ubloxDisableNMEAValSet()
UBLOX_SET_SBAS	配置SBAS	ubloxSetSbas()
UBLOX_SET_PMS	设置电源模式	ubloxSendPowerMode()
UBLOX_MSG_NAV_PVT	启用 PVT 消息	ubloxSetMessageRate()
UBLOX_ACQUIRE_MODEL	设置导航模型	ubloxSendNAV5Message()
UBLOX_SET_NAV_RATE	设置最终导航速率	ubloxSetNavRate()

VALSET 配置（M9+ 模块）

现代 UBLOX 模块采用 VALSET 方法进行配置：

消息速率配置

系统根据模块功能配置特定的消息速率：

• NAV-PVT：M8+模块的主要导航信息
• NAV-SOL：旧版模块的传统解决方案消息
• NAV-POSLLH：位置消息
• NAV-VELNED：速度信息
• NAV-SAT/SVINFO：卫星信息

GPS数据处理

数据接收与解析

GPS 数据处理管道处理多种协议类型，并通过全面的处理链维护状态。

GPS 更新处理

该gpsUpdate()函数src/main/io/gps.c71-85管理完整的 GPS 处理周期：

1. 串行数据接收：从串行端口读取可用字节gpsPort
2. 协议检测：根据协议将数据路由到相应的解析器gpsConfig()->provider
3. 帧验证：验证校验和及消息完整性
4. 解决方案更新：gpsSolutionData_t gpsSol已更新新的位置数据
5. 导航计算：计算衍生导航值

GPS解决方案数据

核心 GPS 数据存储在gpsSolutionData_t gpsSol包含以下内容的文件中：

• 位置：纬度、经度、海拔（厘米）
• 质量指标：HDOP、VDOP、PDOP 值乘以 100
• 精度：水平、垂直和速度精度（单位：毫米）
• 运动：地面速度、三维速度、地面运动轨迹
• 时间：GPS 时间和导航间隔

位置和高度估计

位置估计系统结合了 GPS 和气压计数据，通过提供精确的高度和垂直速度计算calculateEstimatedAltitude()。

高度融合算法

高度源配置

该系统支持多种高度估算模式positionConfig()->altitude_source：

模式	描述	GPS使用情况	晴雨表使用情况
DEFAULT	智能混合	主要（具有信任权重）	备用方案和混合
BARO_ONLY	仅气压计	没有任何	原始资料
GPS_ONLY	仅限 GPS	原始资料	没有任何

GPS信任度计算

GPS信任度是动态计算的。src/main/flight/position.c119-126基于：

• PDOP值：gpsTrust = 100.0f / gpsSol.dop.pdop
• 质量阈值：信任度上限为 90%，始终使用一些晴雨表数据
• 3D修复要求：如果没有有效的3D修复，信任度将降至0。

高度归零和偏移管理

该系统维护多个高度参考点：

• 已解除武装：收集并平滑偏移值，显示绝对高度
• 已启用：应用偏移量进行相对高度计算，连续估算

滤波和平滑

位置估计采用级联PT2滤波器以获得平滑的高度输出：

1. 主要筛选条件：pt2FilterApply(&altitudeLpf, zeroedAltitudeCm)

   • 截止频率：positionConfig()->altitude_lpf / 100.0f赫兹
   • 平滑高度估计

2. 导数滤波器：pt2FilterApply(&altitudeDerivativeLpf, zeroedAltitudeDerivative)

   • 截止频率：positionConfig()->altitude_d_lpf / 100.0f赫兹
   • 平滑垂直速度计算

配置选项

GPS系统通过以下gpsConfig_t结构进行配置：

范围	描述	默认
提供商	GPS 提供商（NMEA、UBLOX、MSP、虚拟）	GPS_UBLOX
sbasMode	SBAS 模式（自动、EGNOS、WAAS、MSAS、GAGAN、无）	SBAS_NONE
自动配置	启用/禁用自动配置	GPS自动配置已开启
自动波特率	启用/禁用自动波特率检测	GPS自动波特率关闭
gps_ublox_acquire_model	动态收购模型	UBLOX_MODEL_STATIONARY
gps_ublox_flight_model	飞行动态模型	UBLOX_MODEL_AIRBORNE_4G
gps更新速率_赫兹	GPS更新频率（赫兹）	10
gps_ublox_use_galileo	启用伽利略卫星	错误的
gps_set_home_point_once	仅设置一次返航点	错误的
gps_use_3d_speed	使用三维速度而非地面速度	错误的
sbas_integrity	启用 SBAS 完整性	错误的
gps_ublox_utc_standard	UTC时间同步标准	UBLOX_UTC_STANDARD_AUTO
nmea自定义命令	自定义 NMEA 命令	""

动态导航模型

UBLOX GPS 模块支持针对不同类型运动的不同动态模型：

模型	描述	用例
UBLOX_MODEL_PORTABLE	便携式应用程序	低动态手持设备
UBLOX_MODEL_STATIONARY	固定应用	固定位置、定时应用
UBLOX_MODEL_PEDESTRIAN	行人申请	步行，缓慢移动
UBLOX_MODEL_AUTOMOTIVE	汽车应用	陆地车辆应用
UBLOX_MODEL_AT_SEA	海洋应用	海事应用
UBLOX_MODEL_AIRBORNE_1G	以小于 1g 的加速度在空中飞行	低动力飞机
UBLOX_MODEL_AIRBORNE_2G	空中加速小于2g	中等动力飞机
UBLOX_MODEL_AIRBORNE_4G	以小于 4g 的加速度飞行	高动力飞机

Betaflight默认UBLOX_MODEL_STATIONARY用于采集数据和飞行。UBLOX_MODEL_AIRBORNE_4G

SBAS 配置

卫星增强系统 (SBAS) 提供 GPS 校正数据以提高定位精度。Betaflight 支持：

SBAS模式	描述	卫星
SBAS_AUTO	自动选择	所有可用卫星
SBAS_EGNOS	欧洲GNSS导航叠加服务	PRN123、PRN126、PRN136
SBAS_WAAS	广域增强系统（美国）	PRN131、PRN133、PRN135、PRN138
SBAS_MSAS	多功能卫星增强系统（日本）	PRN129、PRN137
SBAS_GAGAN	GPS辅助地理增强导航（印度）	PRN127、PRN128、PRN132
SBAS_NONE	无 SBAS	没有任何

与其他系统的集成

该GPS系统可与Betaflight的其他几个系统集成：

GPS救援

提供信号丢失时返航功能的位置数据。

OSD

在 FPV 画面上显示位置信息、到家距离和其他 GPS 指标。

遥测

通过遥测协议将 GPS 数据传输到远程系统。

位置估计

结合 GPS 高度和气压计数据，提高高度估算精度。

GPS状态指示灯

GPS状态通过车载LED系统显示：

• 当 GPS 定位有效时，信号稳定。
• GPS 初始化但没有有效定位时闪烁

这样无需查看 OSD 或遥测数据即可直观地显示 GPS 状态。

虚拟GPS

对于模拟器环境，Betaflight 包含一个虚拟 GPS 实现，允许通过 MSP 或直接函数调用来提供 GPS 数据。这主要用于在没有实际硬件的情况下测试依赖于 GPS 的功能。