企业官网建设流程全解析

• 嵌入式
• 物联网
• 机器人
• 自动驾驶
• 智能硬件

本文面向 PX4 飞控开发与调试人员，围绕 PX4-Autopilot 中的 uORB 消息SensorGyroFft展开：它承载 IMU 陀螺仪三轴信号的 FFT 频谱峰值信息，是 PX4 动态陷波滤波器（Dynamic Notch Filter）基于 FFT 模式进行振动抑制的核心数据源。读完本文，你将掌握该消息全部字段的含义与单位、其在 gyro_fft 模块 中的生成原理，以及下游 VehicleAngularVelocity 模块 如何消费这些峰值频率来实时整定陷波滤波器。

消息概览：发布主题与定位

SensorGyroFft是一条 uORB 消息，发布主题名为sensor_gyro_fft。它的定位是周期性对外发布陀螺仪原始信号的频谱分析结果，具体包括三轴各自的峰值频率与峰值信噪比（SNR），供姿态估计链路中的滤波模块使用。消息定义源文件位于 msg/SensorGyroFft.msg，本文所依据的字段文档位于 docs/en/msg_docs/SensorGyroFft.md。

从 uORB 的角度看，SensorGyroFft属于"传感器派生数据"类消息：它并不直接对应某个硬件寄存器，而是由软件模块对sensor_gyro（或sensor_gyro_fifo）原始数据做实时 FFT 运算后生成的二次消息。因此它的时间戳语义、设备 ID 语义与原始陀螺仪消息保持一致，方便消费方做数据关联。

字段全解：从时间戳到三轴峰值

该消息共 16 个字段，可分为"时间与设备标识"、"频谱元信息"、"三轴峰值结果"三组。完整字段如下：

时间戳与设备标识

• timestamp：与 PX4 绝大多数 uORB 消息一致，表示消息发布时刻，单位微秒，基准为系统启动。
• timestamp_sample：原始陀螺仪采样的时间戳。由于 FFT 需要累积一整帧（例如 512 个采样点）后才处理一次，timestamp_sample与timestamp之间存在帧累积延迟，消费方做数据对齐时应以timestamp_sample为准。
• device_id：标识数据来自哪一颗 IMU。PX4 支持多 IMU 冗余，该字段允许下游模块（如动态陷波滤波）校验 FFT 数据确实来自当前选定的陀螺仪，避免不同传感器数据混用。

频谱元信息：采样率与频率分辨率

• sensor_sample_rate_hz：陀螺仪实际采样率。在 GyroFFT.cpp 中，该值在发布时被赋为模块内部记录的_gyro_sample_rate_hz（默认按 8 kHz 初始化，见 GyroFFT.hpp），实际会随所选传感器与 FIFO 配置更新。
• resolution_hz：FFT 频率分辨率，计算公式为采样率 / FFT 点数，即_gyro_sample_rate_hz / _imu_gyro_fft_len（见 GyroFFT.cpp）。例如采样率 8000 Hz、FFT 点数 512 时，分辨率为约 15.6 Hz。

这两个字段对下游极其关键：动态陷波滤波器需要确认 FFT 的采样率与其内部滤波器采样率一致（误差在 2% 以内才采用数据），并用resolution_hz来设定陷波滤波器的带宽下限约束。

三轴峰值：频率与 SNR

消息为 X、Y、Z 三轴各保留了3 个峰值槽位，即最多报告 3 个显著振动峰值。三个轴共享同一组数据结构（float32[3]），因此即使某一轴只检测到 1 个峰值，剩余槽位也会保留在数组中，语义上对应"该轴未检测到更多显著峰值"。从源码看，峰值数量上限由编译期常量MAX_NUM_PEAKS决定，它直接取自消息数组大小：

static constexpr int MAX_NUM_PEAKS = sizeof(sensor_gyro_fft_s::peak_frequencies_x) / sizeof(sensor_gyro_fft_s::peak_frequencies_x[0]);

（见 GyroFFT.hpp，该值即 3。）

• peak_frequencies_x/y/z：各轴检测出的峰值频率（Hz）。
• peak_snr_x/y/z：对应峰值的信噪比（SNR），用于评估峰值显著程度；信噪比不足的谱线不会作为有效峰值上报。

需要特别说明的是，当某轴在某次检测中未能稳定确认峰值时，消息中的对应槽位可能填入NAN（见 GyroFFT.cpp），消费方在读取时必须做有限值（finite）与正数判断，不能假定数组内所有元素始终有效。

数据来源：gyro_fft 模块的生成链路

SensorGyroFft由独立的gyro_fft模块生成并发布。模块工作于 PX4 工作队列（ScheduledWorkItem，挂载于hp_default高优先级队列），发布句柄为uORB::Publication<sensor_gyro_fft_s>（见 GyroFFT.hpp）。

输入选择：sensor_gyro 与 sensor_gyro_fifo

模块订阅sensor_selection以获知当前选定的陀螺仪设备 ID，然后优先选择sensor_gyro_fifo（若该传感器支持 FIFO 批量采样），否则退回到sensor_gyro单点采样（见 GyroFFT.cpp 附近的传感器选择逻辑）。两种输入最终都转换为int16_t采样序列，写入三轴数据缓冲。

FFT 计算过程

一次完整的 FFT 计算流程（见 GyroFFT.cpp 的Update()与 FindPeaks）大致如下：

1. 数据累积：把每次收到的陀螺仪采样写入环形缓冲_gyro_data_buffer_{x,y,z}，累积满一个 FFT 帧长度_imu_gyro_fft_len后触发一次 FFT。
2. 加窗：使用预计算的 Hanning 窗（_hanning_window）对时域数据加权（arm_mult_q15），降低频谱泄漏。Hanning 窗在模块init()中按0.5f * (1.f - cosf(2πn/(N-1)))生成（见 GyroFFT.cpp）。
3. 实序列 FFT：调用 CMSIS-DSP 的arm_rfft_q15完成定点 q15 格式的实数 FFT（FFT 长度可为 256 / 512 / 1024，见 GyroFFT.cpp 的枚举分发；源码中还保留了 128 / 2048 / 4096 / 8192 等长度的注释代码）。
4. 帧重叠：每帧处理完毕后仅丢弃前 1/4 数据（3/4 重叠），保证谱更新平滑（见 GyroFFT.cpp）。
5. 峰值搜索：在频率轴每个 bin 上计算幅度sqrt(real² + imag²)，只在[IMU_GYRO_FFT_MIN, IMU_GYRO_FFT_MAX]频率窗内搜索最大峰，找到后连同左右邻 bin 一起清零，再继续找下一个峰，如此循环最多找满 3 个峰（见 GyroFFT.cpp）。
6. 峰值确认与平滑：峰需要满足 SNR 阈值才会上报；已上报的峰还会与历史槽位做频率匹配，使同一振动峰在不同帧之间保持槽位稳定，并经过math::MedianFilter<float, 7>中值滤波抑制抖动（见 GyroFFT.hpp）。

发布前填充元信息

在Publish()中，模块将采样率与分辨率写入消息：

_sensor_gyro_fft.sensor_sample_rate_hz = _gyro_sample_rate_hz; _sensor_gyro_fft.resolution_hz = _gyro_sample_rate_hz / _imu_gyro_fft_len;

（见 GyroFFT.cpp。）随后_sensor_gyro_fft_pub.publish(_sensor_gyro_fft)将整条消息发布到sensor_gyro_fft主题。

下游消费：动态陷波滤波器的 FFT 模式

SensorGyroFft最主要的消费方是姿态角速度模块VehicleAngularVelocity。在该模块的 UpdateDynamicNotchFFT 中：

• 仅当参数IMU_GYRO_DNF_EN开启了FFT位（_param_imu_gyro_dnf_en.get() & DynamicNotch::FFT）时才订阅处理该消息；
• 接收后先校验device_id与当前选定陀螺仪一致、消息时效在DYNAMIC_NOTCH_FITLER_TIMEOUT以内，并且sensor_sample_rate_hz与滤波器内部采样率的偏差小于 2%，满足条件才使用数据；
• 随后以resolution_hz约束陷波带宽（math::constrain(resolution_hz, 8.f, 30.f)），并逐轴逐峰读取peak_frequencies_x/y/z：频率大于下限（源码中暂为 10 Hz）的峰用于整定对应轴的动态陷波滤波器（nf.setParameters(_filter_sample_rate_hz, peak_freq, bandwidth)），否则禁用该陷波器。

这条链路回答了"消息字段为什么这样设计"：频率分辨率字段用于带宽约束，采样率字段用于一致性校验，设备 ID 用于多 IMU 关联，而三轴各 3 个峰值槽位则对应三个轴最多各 3 个动态陷波滤波器。

关联配置参数

SensorGyroFft的生成行为由gyro_fft模块的 5 个参数控制，定义于 parameters.yaml：

几点实践提示：

• FFT 点数与分辨率：resolution_hz = 采样率 / FFT 点数。点数越大频率分辨率越高，但计算与内存开销也越大，且每帧需要累积更多采样、响应延迟变长。源码中 GyroFFT.cpp 实际只启用 256 / 512 / 1024 三种长度（4096 等分支被注释），配置超出范围时模块会打印错误并回退到 256。
• 频率窗口：IMU_GYRO_FFT_MIN/MAX限定峰值搜索窗口，默认 30 ~ 150 Hz 覆盖了多旋翼电机/螺旋桨的常见振动频段；窗口应结合机型实际振动特性调整，避免搜索到无关谱线。
• SNR 阈值：IMU_GYRO_FFT_SNR默认 10（即 10 dB 量级的阈值语义），阈值过低会引入噪声峰，过高则会漏报真实振动峰。此外，峰值确认逻辑中还内置了最小 SNR 1 的"已确认峰保持"规则（GyroFFT.cpp）。
• 上述参数大多标注reboot_required: true，修改后需重启飞控生效。

日志与调试

SensorGyroFft被登记为可选日志主题：在 logged_topics.cpp 中通过add_optional_topic("sensor_gyro_fft", 50)注册，最大记录速率 50 Hz。开发者可通过logger模块的可选主题配置将其加入飞行日志，用 Flight Review / ulog 分析工具查看三轴峰值频率随时间的变化曲线，直接判断机体振动峰是否被正确识别、动态陷波滤波器是否跟踪到目标频率。

此外，gyro_fft模块在 GyroFFT.hpp 中注册了若干性能计数器（gyro_fft: cycle、gyro_fft: cycle interval、gyro_fft: FFT），可在 NSH 终端执行gyro_fft status查看 FFT 计算耗时与周期，用于评估 FFT 点数配置对 CPU 负载的影响。

小结

SensorGyroFft是 PX4 传感器频谱分析链路的枢纽消息：gyro_fft模块对选定 IMU 的三轴原始信号做加窗 FFT 与峰值搜索，将频率分辨率、采样率和三轴峰值频率/SNR 打包发布；VehicleAngularVelocity模块校验设备 ID、时效与采样率后，依据这些峰值实时整定三轴动态陷波滤波器，从而在线抑制电调、螺旋桨等旋转部件引起的振动。理解该消息的字段语义、生成链路与消费条件，是排查振动抑制失效、动态陷波不跟踪等问题的基础。

进一步阅读：消息源定义 msg/SensorGyroFft.msg、生产者模块 src/modules/gyro_fft/GyroFFT.cpp、参数定义 src/modules/gyro_fft/parameters.yaml、消费者实现 src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp。

• 嵌入式
• 物联网
• 机器人
• 自动驾驶
• 智能硬件