【无人机】MavLink通讯协议的回调解析函数及状态机

【无人机】MavLink通讯协议的回调解析函数及状态机

MavLink

MavLink是一种常用于无人机通讯领域的协议栈
开源链接

其常用1.0和2.0协议栈数据结构如下：

签名报文格式：

MavLink解析函数

在官方提供的API中 常用mavlink_parse_char函数进行解析

/*** This is a convenience function which handles the complete MAVLink parsing.* the function will parse one byte at a time and return the complete packet once* it could be successfully decoded. This function will return 0 or 1.** Messages are parsed into an internal buffer (one for each channel). When a complete* message is received it is copies into *r_message and the channel's status is* copied into *r_mavlink_status.** @param chan     ID of the channel to be parsed.*                 A channel is not a physical message channel like a serial port, but a logical partition of*                 the communication streams. COMM_NB is the limit for the number of channels*                 on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows* @param c        The char to parse** @param r_message NULL if no message could be decoded, otherwise the message data* @param r_mavlink_status if a message was decoded, this is filled with the channel's stats* @return 0 if no message could be decoded or bad CRC, 1 on good message and CRC** A typical use scenario of this function call is:** @code* #include <mavlink.h>** mavlink_status_t status;* mavlink_message_t msg;* int chan = 0;*** while(serial.bytesAvailable > 0)* {*   uint8_t byte = serial.getNextByte();*   if (mavlink_parse_char(chan, byte, &msg, &status))*     {*     printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);*     }* }*** @endcode*/
MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
{uint8_t msg_received = mavlink_frame_char(chan, c, r_message, r_mavlink_status);if (msg_received == MAVLINK_FRAMING_BAD_CRC ||msg_received == MAVLINK_FRAMING_BAD_SIGNATURE) {// we got a bad CRC. Treat as a parse failuremavlink_message_t* rxmsg = mavlink_get_channel_buffer(chan);mavlink_status_t* status = mavlink_get_channel_status(chan);_mav_parse_error(status);status->msg_received = MAVLINK_FRAMING_INCOMPLETE;status->parse_state = MAVLINK_PARSE_STATE_IDLE;if (c == MAVLINK_STX){status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;rxmsg->len = 0;mavlink_start_checksum(rxmsg);}return 0;}return msg_received;
}

在这里 官方提供的方式就是每次往这个函数输入一个字节 直到返回1表示解析成功

回调函数状态机

为了每次不占用主线程 且保证接收正常、兼容MCU等串口回调方式
可以使用状态机来进行
状态机V2版本可为：

static uint8_t mavlink_callback_status = 0;
static uint32_t mavlink_callback_flag = 0;
static uint8_t mavlink_callback_buf[512]={0};void reset_mavlink_callback(void)
{mavlink_callback_status = 0;mavlink_callback_flag = 0;memset(mavlink_callback_buf,0,sizeof(mavlink_callback_buf));
}uint32_t mavlink_callback(uint8_t dat)
{switch (mavlink_callback_status) {case 0:{if(dat == 0xFD){mavlink_callback_buf[0] = dat;mavlink_callback_flag = 1;mavlink_callback_status = 1;}break;}case 1:  // 读取载荷长度{mavlink_callback_buf[mavlink_callback_flag] = dat;mavlink_callback_flag++;// 检查消息总长度是否合法 (10字节头尾 + 载荷 + 2字节校验)if(10 + dat + 2 > sizeof(mavlink_callback_buf)) {reset_mavlink_callback();return 0;}mavlink_callback_status = 2;break;}case 2:  case 3:  case 4: {mavlink_callback_buf[mavlink_callback_flag] = dat;mavlink_callback_flag++;mavlink_callback_status++;break;}case 5:  case 6:  {if(!dat){reset_mavlink_callback();break;}}case 7:  case 8:  case 9:  {mavlink_callback_buf[mavlink_callback_flag] = dat;mavlink_callback_flag++;mavlink_callback_status++;break;}case 10: // 处理有效载荷{mavlink_callback_buf[mavlink_callback_flag] = dat;mavlink_callback_flag++;// 检查是否完成载荷接收if(mavlink_callback_flag >= 10 + mavlink_callback_buf[1]) {mavlink_callback_status = 11;}break;}case 11: // 校验和低字节case 12: // 校验和高字节{mavlink_callback_buf[mavlink_callback_flag] = dat;mavlink_callback_flag++;mavlink_callback_status++;if(mavlink_callback_status > 12) {return mavlink_callback_flag;  // 完整帧接收完成}break;}default:reset_mavlink_callback();break;}// 安全边界检查if(mavlink_callback_flag >= sizeof(mavlink_callback_buf)) {reset_mavlink_callback();return 0;}return 0;
}

如果是V1版本 则需要改起始位 以及后面的状态机移位

调用方法：

if(n>0){printf("\nnum:%d size:%d : \n",row,n);uint32_t mavlink_len = 0;mavlink_message_t pos_msg;mavlink_status_t status;for (ssize_t i = 0; i < n; i++){mavlink_len = mavlink_callback(read_buf[i]);if(mavlink_len){for (ssize_t j = 0; j < mavlink_len; j++){if (mavlink_parse_char(MAVLINK_COMM_0, mavlink_callback_buf[j], &pos_msg, &status)){if (pos_msg.msgid == MAVLINK_MSG_ID_POSITION_ESTIMATE){mavlink_position_estimate_t position_estimate;mavlink_msg_position_estimate_decode(&pos_msg, &position_estimate);printf("mcu: %lu us, yaw = %.5f\n",position_estimate.usec, position_estimate.yaw);dxfAttitude.pitch = position_estimate.pitch;dxfAttitude.roll = position_estimate.roll;dxfAttitude.yaw = position_estimate.yaw;}}}reset_mavlink_callback();}}}

附录：crazyfile固件代码功能解读及其线程调度

在无人机开源领域 有个名为crazyfile的无人机软件架构
地址为：github.com/bitcraze/crazyflie-firmware

线程分配

1. 主线程systemTask

main函数建立主线程systemTask，该线程完成了其他各线程的初始化及调度。
默认主要实现的功能：

1. LED初始化
2. 定时器初始化
3. I2C初始化
4. 透传初始化及其线程分配
5. high-levels modules初始化及其线程分配
6. 卡尔曼滤波器初始化及其线程分配
7. 内存初始化
8. 系统链路数据包接收队列开启及其扩展板子系统初始化
9. SoundTimer软件定时器
10. CRTP内存队列模块初始化及其内存管理线程
11. 系统各模块测试及其log信息打印
12. 测试通过则开启系统、使能看门狗等，随后进入到workerLoop
13. 测试未通过则重新进行systemTest，若通过，可使用systemTest强制启动；若未通过则亮红灯，并进入到workerLoop
    其余几项由宏定义选择性调用：
14. 队列监控软件定时器
15. 串口1的debug打印初始化
16. errorUkfTask线程，误差状态无迹卡尔曼滤波器初始化
17. proximity线程，靠近传感器及滑动窗口初始化

2. 主要线程或循环任务

线程名称	优先级或循环时间	创建函数	宏定义选择开启	功能描述	所属分类
systemTask	SYSTEM_TASK_PRI	systemLaunch	-	系统主控线程，负责总初始化流程	主线程
usblinkTask	USBLINK_TASK_PRI	usblinkInit	-	USB通信协议处理	基础服务线程
sysLoadTask	TIMER_PERIOD（默认1000）	sysLoadInit	-	系统负载监控	-
crtpTask （crtpTxTask、crtpRxTask）	CRTP_TX_TASK_PRI CRTP_RX_TASK_PRI	crtpInit	-	CRTP服务	通信协议栈
commTask （crtpSrvTask、platformSrvTask、syslinkTask、paramTask）	CRTP_SRV_TASK_PRI PLATFORM_SRV_TASK_PRI SYSLINK_TASK_PRI PARAM_TASK_PRI	commInit	-	CRTP无线通信协议处理	-
commanderTask （crtpCommanderHighLevelTask）	CMD_HIGH_LEVEL_TASK_PRI	commanderInit	-	遥控指令解析与分发	-
proximityTask	PROXIMITY_TASK_PRI	proximityInit	PROXIMITY_ENABLED	接近传感器数据处理	传感器处理线程
estimatorKalmanTask （kalmanTask）	KALMAN_TASK_PRI	estimatorKalmanTaskInit	CONFIG_ESTIMATOR_KALMAN_ENABLE	卡尔曼滤波算法	核心算法线程
errorUkfTask	ERROR_UKF_TASK_PRI	errorEstimatorUkfTaskInit	CONFIG_ESTIMATOR_UKF_ENABLE	UKF估计算法	-
memTask	MEM_TASK_PRI	crtpMemInit	-	内存管理服务	设备管理
stabilizerTask	STABILIZER_TASK_PRI	stabilizerInit	-	姿态稳定控制	-
passthroughTask	PASSTHROUGH_PRI	passthroughInit	-	电机电调烧录控制	扩展模块
appTask	CONFIG_APP_PRIORITY	appInit	CONFIG_APP_ENABLE	用户应用（未定义）	-
zRangerTask （zRanger2Task）	ZRANGER_TASK_PRI （ZRANGER2_TASK_PRI）	zRangerInit （由DECK_DRIVER注册） （zRanger2Init）	-	激光测距扩展板	-
SoundTimer	10ms	soundInit	-	蜂鸣器音效控制	定时器任务
queueMonitorTimer	TIMER_PERIOD（默认1000）	queueMonitorInit	CONFIG_DEBUG_QUEUE_MONITOR	队列监控	-

主要线程及其功能

1. stabilizerTask

飞控系统控制相关的核心线程。
在系统启动同步后进入1kHz实时控制循环，首先通过sensorsWaitDataReady()等待IMU传感器数据就绪，调用sensorsAcquire()获取加速度计/陀螺仪原始数据；随后进行状态估计（stateEstimator()解算飞行姿态）、处理遥控指令生成设定点（commanderGetSetpoint()），通过安全监控模块（supervisorUpdate()）检查电池/通信状态，调用PID控制算法（controller()）计算电机推力，执行带电压补偿的PWM输出（controlMotors()）；最后压缩日志数据（compressState()）并通过xSemaphoreGive()释放信号量触发频率监控任务（rateSupervisorTask），同时处理避障指令覆盖和异常状态急停，完成从数据采集到电机驱动的全闭环控制。
这里会建立一个子线程rateSupervisorTask：飞控系统的稳定器循环监控线程，在系统启动后创建监控信号量（xRateSupervisorSemaphore），通过阻塞等待实时监测稳定器主循环（1000Hz）的执行间隔，当检测到超时（2秒未执行）或频率偏移（±3Hz）时触发系统警告，并通过断言强制停机保障飞行安全，同时智能区分传感器暂停状态（isSensorsSuspended）避免误报警
如果开启避障功能，proximityTask线程会进行近距离测量，在stabilizerTask线程中会调用collisionAvoidanceUpdateSetpoint，进行避障操作。

2. kalmanTask（estimatorKalmanTask）

飞控系统的卡尔曼滤波核心线程。
在系统启动后进入实时状态估计循环，首先通过xSemaphoreTake()等待来自稳定器线程的信号量触发计算周期，获取加速度计和陀螺仪的子采样数据（axis3fSubSamplerFinalize），执行卡尔曼预测步骤（kalmanCorePredict）推算飞行器状态；随后处理队列中的多源测量数据（TDOA/光流/TOF等），调用kalmanCoreUpdateWith*系列函数进行测量更新；最后通过协方差检查（kalmanSupervisorIsStateWithinBounds）确保估计值有效性，并通过互斥锁保护将最终状态（kalmanCoreExternalizeState）输出给稳定器线程。整个流程严格控制在100Hz频率，实现从传感器原始数据到高精度姿态/位置估计的实时解算。

3. CRTP（crtpTask、commTask、commanderTask）

crtpTask

实时传输协议（CRTP）的初始化核心。
在系统启动阶段创建双线程通信架构（TX/RX任务），初始化全局发送队列（txQueue）和端口接收队列（queues[16]），建立基于优先级队列的通信调度机制，为后续无线/USB等通信链路提供协议栈支持，同时集成统计模块实时监控数据吞吐量。
这里建立了两个子线程，crtpTxTask和crtpRxTask。

1. crtpTxTask：CRTP 协议栈的数据发送核心线程。从全局发送队列（txQueue）阻塞接收待发送数据包，通过动态绑定的通信链路（无线电/USB）执行数据发送，在链路拥塞时自动进行10ms间隔重试，同时统计发送速率并通过日志系统实时上报，保障控制指令和日志数据的可靠传输。
2. crtpRxTask：CRTP 协议栈的数据接收核心线程。通过动态绑定的通信链路（无线电/USB）持续接收数据包，根据端口号（p.port）将数据包分发至对应接收队列（queues[16]）或触发注册的回调函数（callbacks[16]），同时统计接收速率并通过日志系统实时上报，实现多通道数据解耦处理。

commTask

飞控系统的通信调度核心线程。
在系统启动时初始化多协议通信栈（CRTP/USB/Radio），建立与地面站/遥控器的无线连接，通过优先级调度管理控制指令（CRTP包）、参数同步、日志传输等多类数据流。其实时监听UART/2.4GHz无线电等硬件接口，解析并分发来自外部的飞行控制指令至指挥官模块，同时将传感器数据/状态信息通过压缩协议回传地面站，并处理通信中断时的故障切换逻辑。
这里新建了四个线程，分别为crtpSrvTask、platformSrvTask、syslinkTask、paramTask。

1. crtpSrvTask：CRTP 协议服务核心线程，在系统启动后创建专用通信队列（CRTP_PORT_LINK），通过阻塞式接收（crtpReceivePacketBlock）监听来自地面站的 CRTP 指令包，根据数据包通道类型执行回显测试（Echo）、设备标识查询（Source）或数据接收黑洞（Sink）操作。其实时处理链路层控制指令，支持通过 echoDelay 参数动态配置回显延迟，并通过互斥锁保障多线程通信安全，完整实现 CRTP 协议的基础服务功能。
2. platformSrvTask：飞控系统的平台服务核心线程，在系统启动后创建专用服务端口（CRTP_PORT_PLATFORM），通过阻塞式接收（crtpReceivePacketBlock）监听来自地面站的平台控制指令，根据数据包通道类型执行射频模式设置（ContinuousWave）、系统上电状态管理（armSystem）、固件版本查询（versionCommand）等关键操作。其实时处理硬件级控制请求，并通过Syslink协议与底层硬件交互，保障系统安全状态切换和平台信息透明化。
3. syslinkTask：飞控系统的无线通信核心线程。在系统启动后创建专用通信队列，通过阻塞接收（uartslkGetPacketBlocking）持续监听NRF51协处理器发来的Syslink协议数据包，根据协议头（SYSLINK_RADIO_GROUP/SYSLINK_PM_GROUP等）将数据包路由至对应子系统（无线电控制/电源管理/调试信息等），同时通过互斥锁保障多线程通信安全，实现STM32与NRF51间全双工通信。
4. paramTask：飞控系统的参数管理核心线程。在系统启动后创建专用参数端口（CRTP_PORT_PARAM），通过阻塞接收（crtpReceivePacketBlock）解析来自地面站的参数操作指令，执行参数目录查询（TOC）、实时读写（Read/Write）、持久化存储（Persistent Store）等操作，并通过互斥锁保障多线程访问安全，完整实现飞控参数的动态配置与生命周期管理。

commanderTask

crtpCommanderHighLevelTask 是高层指令处理核心线程。
通过CRTP协议接收地面站的高层飞行指令（起飞/降落/轨迹跟踪），解析后调用运动规划器生成平滑轨迹。其实时响应包括：垂直起降速度控制、三维空间点对点运动插值、预定义轨迹加载执行，并通过互斥锁保障与状态估计线程的安全交互。

4. proximityTask

飞控系统的近距感知核心线程，用于测距数据上报。
在系统启动后初始化滑动窗口滤波器（proximitySWin），以 20Hz 固定频率（PROXIMITY_TASK_FREQ）通过 maxSonarReadDistance() 读取 MaxBotix 传感器原始距离数据，执行滑动窗口均值滤波（proximitySWinAdd）和中值滤波（proximitySWinMedian），最终输出三重有效距离值（瞬时/均值/中值）并通过日志系统实时上传，为避障模块提供可靠的环境感知数据。
如果开启避障功能，则在stabilizerTask线程中会调用collisionAvoidanceUpdateSetpoint，进行避障操作。

5. zRangerTask（这里用的是zRanger2Task）

该线程的初始化函数由DECK_DRIVER注册。

zRangeTask

zranger主要是使用VL53L0X模块来进行测距。
VL53L0X 激光测距模块的专用驱动线程。在系统启动后初始化 TOF 传感器（vl53l0xInit），配置连续测量模式（100Hz 采样率），通过 vl53l0xReadRangeContinuousMillimeters() 获取原始距离数据，执行异常值过滤（>3m 视为无效），最终将有效测量值（单位：米）通过 rangeEnqueueDownRangeInEstimator() 注入卡尔曼滤波器，同时更新全局距离状态供避障模块使用。

zRanger2Task

对于zranger2，则是使用VL53L1X模块来进行测距。该线程所使用的测距结果应用在无人机的正下方方向。
主要实现功能为：

1. 传感器初始化：通过 vl53l1xInit 函数初始化 VL53L1X 传感器（在 zRanger2Init 中完成），配置中距离模式（VL53L1_DISTANCEMODE_MEDIUM）和 25ms 测量周期（VL53L1_SetMeasurementTimingBudgetMicroSeconds），实现 40Hz 的实际采样率
2. 周期性测量：每 25ms（由 M2T(25) 定时）通过 zRanger2GetMeasurementAndRestart 获取原始毫米级数据，该函数内部通过 VL53L1_GetRangingMeasurementData 读取传感器数据，并在每次测量后自动重启设备（VL53L1_StopMeasurement + VL53L1_StartMeasurement）
3. 异常值过滤：通过 RANGE_OUTLIER_LIMIT (5000mm/5m) 阈值过滤传感器异常值（代码中注释提到传感器最大有效量程为 5m，但实际异常值常表现为 >8m）
4. 数据预处理：将毫米级原始数据转换为米（0.001f），并基于指数噪声模型计算动态标准差（expStdA(1+exp(expCoeff*(d-expPointA)))），该模型在 2.5m 时标准差 0.0025m，4m 时增长到 0.2m
5. 数据融合：通过 rangeEnqueueDownRangeInEstimator 将带噪声评估的距离值注入卡尔曼滤波器（在 src/modules/src/range_kalman.c 中实现），同时通过 rangeSet 更新全局 rangeDown 状态（定义在 src/modules/interface/range.h），供避障模块和高度控制模块使用

6. passthroughTask

飞控系统的 ESC（无刷电机电调） 固件烧录核心线程。在收到上位机烧录指令后（通过 USB VCP 虚拟串口），首先与 BLHeli 配置工具进行 MSP 协议握手（blHeliConfigHandshake），随后暂停 UART 接收中断和传感器数据采集（uartslkPauseRx/sensorsSuspend），初始化 4-way 接口进入 ESC 编程模式（esc4wayInit），最终通过位翻转协议（Bit-Banging）完成 ESC 固件烧录或参数配置，全程保障飞行器处于安全状态。

7. appTask（未定义）

该线程为用户自己定义的app线程，目前尚未定义。

主要模块功能说明

1. MCU驱动层

原本的MCU驱动层相关代码都被抽象到了应用层，比如I2C。
驱动层相关的配置代码与MCU有关，其代码多在src/drivers/src目录下
若引脚发生变化，芯片选型改变等，就需要重新构建驱动层相关部分的代码结构。
以下为常见使用的几个外设。
LED
抽象后的应用层操作函数为ledSet。
I2C
其驱动层代码在i2c_drv.c中，初始化时的应用层则在i2cdev中。
通过调用底层驱动i2cdrvInit()初始化指定I2C总线（I2C1/I2C3）的时钟、GPIO复用模式和中断配置，建立总线仲裁机制与超时重试策略，为上层传感器/扩展设备提供标准化的读写接口。
UART
其驱动层代码在uart_syslink中。在commInit、passthroughInit等中都有对UART的调用。
SPI
在deck、nf24l01、bmi088、fatfs等中，均有使用。

2. 抽象层

抽象层代码则在src/hal/src目录下，其主要实现的是相关传感器、外部芯片的数据读取、调度等。
抽象层需要调用到驱动层相关函数。
如syslink、sensors、imu等。

3. CRTP通讯协议

通讯协议架构：

CRTP（应用层协议） ↔ radiolink（传输层） ↔ syslink（硬件链路层） ↔ NRF51（射频芯片与STM32交互）

驱动层通过radiolinkOp结构体实现多链路兼容，当调用radiolinkSendCRTPPacket()时，将CRTP数据包封装为Syslink协议格式，通过NRF51射频模块发送，同时crtpRxTask()线程以100Hz频率轮询接收队列，根据数据包头的端口号（p.port字段）将数据分发至对应处理通道。
CRTP通讯协议主要由CRTPPort标志及其crtpReceivePacketBlock函数来实现通讯接收，最终接收的数据存放在CRTPPacket类型变量中。
在进行接收之前要通过crtpInitTaskQueue添加CRTPPort标志到队列中。
若是要新增其他的通讯功能，则可以定义一个新的标志，并且通过一个新建的线程来接收该标志的数据。
在初始化中，USB优先通讯。

    usblinkInit();  // USB链路优先初始化sysLoadInit();#if CONFIG_ENABLE_CPXcpxlinkInit();  // 备选通信协议#endifcrtpInit();     // 初始化无线链路

• USB插入时优先使用USB链路
• USB断开后自动回退到无线链路
• 可通过crtpSetLink()强制指定链路

crtpSetLink(usblinkGetLink());  // 强制使用USB
crtpSetLink(radiolinkGetLink());// 强制使用无线

开发注意事项：

1. 端口分配：飞行控制指令强制使用PORT0，参数传输使用PORT2，自定义功能应从PORT4开始
2. 数据安全：发送前需检查crtpGetFreeTxQueuePackets()防止队列溢出，关键指令使用crtpSendPacketBlock()
3. 链路切换：通过crtpSetLink(usblinkGetLink())可临时启用USB-CDC调试链路
4. 射频参数：修改radiolinkSetChannel(100)可切换至100号频段（需地面站同步）

4. DECK

DECK是用来进行扩展设备管理的。
通过物理接口检测和初始化扩展板卡（如激光测距/光流传感器/UWB定位模块）。在系统启动阶段（systemTask）调用deckInit()遍历注册的DECK驱动，通过VID/PID识别已连接硬件，初始化对应外设（如zRangerInit()初始化VL53L0X激光传感器并创建数据采集任务），声明资源占用（如DECK_USING_IO_1），最终将扩展硬件数据集成到飞控系统（如通过rangeEnqueueDownRangeInEstimator()注入高度测量值）。开发者通过定义DECK_DRIVER结构扩展新硬件，系统自动处理热插拔检测和资源冲突检查。
其主要实现如下：

// 硬件扩展初始化流程
void deckInit() {deckDriverCount();              // 统计注册的DECK驱动deckInfoInit();                 // 解析扩展板硬件IDfor (i=0; i<nDecks; i++) {     // 遍历检测到的扩展板DeckInfo *deck = deckInfo(i);if (deck->driver->init) {   // 调用驱动初始化函数deck->driver->init(deck);// 示例：zRangerInit()初始化激光测距模块}}
}// 典型DECK驱动结构定义（zranger.c）
DECK_DRIVER(zranger_deck) = {.vid = 0xBC,                   // 厂商ID.pid = 0x03,                   // 产品ID.name = "bcZRanger",           // 驱动名称.usedGpio = DECK_USING_IO_1,   // 声明GPIO占用.init = zRangerInit,            // 初始化入口.test = zRangerTest             // 自检函数
};

开发注意事项：

1. 资源声明：必须明确定义usedPeriph和usedGpio，如FlowDeck声明DECK_USING_I2C防止总线冲突
2. 任务优先级：传感器任务需低于飞控核心任务，如激光测距任务ZRANGER_TASK_PRI=3
3. 数据集成：通过rangeEnqueueDownRangeInEstimator()将测量值注入状态估计器
4. 参数配置：在PARAM_GROUP中添加可调参数，如测距模块的噪声模型参数expStdB
5. 硬件检测：在deckTest()中实现I2C设备地址扫描，确保硬件连通性
   若要添加新的设备可以：

DECK_DRIVER(new_deck) = {.vid = 0xBC, .pid = 0xFF,                    // 自定义PID.name = "NewSensor",.usedPeriph = DECK_USING_I2C,    // 声明使用I2C外设.init = newSensorInit,           // 初始化函数.test = newSensorTest            // 硬件自检
};PARAM_GROUP_START(deck)
PARAM_ADD_CORE(PARAM_UINT8 | PARAM_RONLY, newDeck, &isInit)
PARAM_GROUP_STOP(deck)LOG_GROUP_START(newSensor)
LOG_ADD(LOG_FLOAT, value, &sensorValue)
LOG_GROUP_STOP(newSensor)

5. 电机驱动

电机通过PWM来进行驱动，其占用的定时器为motorMap数组，驱动层在src/driver/motors中。
驱动层所用到的定时器通道等，在motors_def中被定义，对于不同的电机类型，有不同的驱动层。每一组电机驱动都需要四个电机引脚来完成。
其调用在stabilizerInit中进行，也就是姿态稳定线程。
在power_distribution.c中实现了动力的分配，并调用更改占空比motorsSetRatio函数的方式进行动力控制。
在stabilizerTask中，实现了IMU、姿态检测，以及PID，最后再进行动力分配。
该驱动方式（PWM）适用于普通有刷电机。

另外还有一种DShot模式，适用于BLHeli_32等无刷电调。由CONFIG_MOTORS_ESC_PROTOCOL_DSHOT宏定义控制。
DShot协议是一种专有协议，通过发送16位数据帧（包含油门指令+校验码）实现数字通信，而 PWM 依赖模拟脉冲宽度表征控制量。DShot实时性来说比传统PWM直接控制更高。

在编译时，通过不同的命令生成不同的Config文件，从而实现不同的驱动方式。

6. syslink

主控（STM32）与无线协处理器（NRF51）间的双向通信协议栈，通过UART实现双核间系统级指令与数据交换。
在系统启动阶段syslinkInit()初始化硬件接口并创建高优先级任务syslinkTask()，该任务以阻塞方式接收数据包（如无线电控制信号、电池状态查询、固件版本请求），通过syslinkRouteIncommingPacket()路由至对应处理模块（如radiolink处理2.4G遥控数据、pm管理电源状态），同时支持发送DMA加速的系统指令（如紧急关机SYSLINK_PM_SHUTDOWN）。开发者可通过扩展SYSLINK_GROUP类型实现自定义双核通信协议。
数据包发送通过syslinkSendPacket实现，在syslinkRouteIncommingPacket中处理接收的协议。
与CRTP的通讯则是由radiolinkSendPacket借助syslink完成的。
主要实现功能：

1. 协议转换：将CRTP数据包封装在SysLink的SYSLINK_RADIO_RAW类型数据帧中（0x00组）
2. 物理层传输：通过UART DMA实现双核间高速传输（实测速率2.4Mbps）
3. 数据完整性保障：添加双字节校验和（cksum[0]为累加和，cksum[1]为累加和的累加和）
4. 带宽管理：使用信号量syslinkAccess防止多线程竞争

7. radiolink

radiolink是syslink与CRTP之间的中间层。
通过2.4GHz射频实现飞控与遥控器/地面站的双向数据透传。该模块在系统启动阶段（systemTask）通过radiolinkInit()初始化NRF51协处理器，配置通信参数（信道/速率/地址），建立CRTP协议与SysLink硬件的桥梁。核心流程通过radiolinkSyslinkDispatch()实现：当NRF51通过SysLink发送SYSLINK_RADIO_RAW类型数据包时，模块解包为CRTP格式并存入接收队列（crtpPacketDelivery），同时触发LED状态指示；发送方向通过radiolinkSendCRTPPacket()将CRTP包封装为SysLink协议格式，利用DMA实现最高250kbps的有效载荷传输，内置流量统计（numRxUc/numRxBc）和信号强度监测（rssi），支持点对点及广播模式，实测端到端延迟小于5ms，是Crazyflie实现群体飞行和实时控制的关键无线链路。

8. NRF51

syslink是NRF51与STM32之间的通讯协议栈。其主要就是串口通讯。
NRF51协处理器作为双核架构中的无线通信与系统管理核心，通过SysLink协议与主控STM32实现双向数据交互。其主要功能包括：处理2.4GHz射频通信（收发CRTP数据包）、管理电源状态（电池电压监测/低功耗模式）、执行单总线传感器扫描（通过SYSLINK_OW_GROUP），并通过SYSLINK_DEBUG_GROUP提供硬件调试接口。与SysLink的交互通过UART接口实现，采用DMA传输和双重校验机制（累加和+累加和校验）。

主要传感器及其模块

1. IMU BMI088（SPI）

该设备有两种通讯模式，I2C和SPI都支持，这里使用的是SPI通讯。
该芯片有由两个传感器组成：

• GYRO（陀螺仪）：测量三维角速度（单位：dps）
• ACCEL（加速度计）：测量三维线性加速度（单位：g）
  这两个传感器都是通过SPI通讯，共用一组SPI数据线，但是CS片选不一样。
  相关涉及到的文件如下：

src/
├── hal/
│   ├── src/
│   │   ├── sensors_bmi088_spi.c       # SPI通信底层实现
│   │   └── sensors_bmi088_common.h    # 通用设备初始化接口
├── drivers/
│   ├── bosch/
│   │   ├── interface/
│   │   │   ├── bmi088.h               # 传感器操作API声明
│   │   │   └── bmi088_defs.h          # 寄存器定义和宏(0x40-0x7E关键配置寄存器)
│   │   └── src/
│   │       ├── bmi088_accel.c         # 加速度计数据获取实现
│   │       └── bmi088_gyro.c          # 陀螺仪数据获取实现

寄存器表如下：


在BMI088传感器的SPI协议中，地址的最高位（第7位）用于标识读/写操作。读操作时寄存器地址需与0x80（BMI088_SPI_RD_MASK）进行或操作，写操作时地址需与0x7F（BMI088_SPI_WR_MASK）进行与操作，该机制在bmi088_get_gyro_regs()和bmi088_set_gyro_regs()中实现。
同样也适用于加速度计部分，在bmi088_get_accl_regs()和bmi088_set_accl_regs()中实现。
主要寄存器配置：

// 加速度计关键寄存器
#define BMI088_ACCEL_PWR_CONF_REG       0x7C // 电源模式配置
#define BMI088_ACCEL_RANGE_REG          0x41 // 量程设置(±3/6/12/24g)
#define BMI088_ACCEL_CONF_REG           0x40 // 带宽和ODR配置// 陀螺仪关键寄存器
#define BMI088_GYRO_RANGE_REG           0x0F // 量程(±125/250/500/1000/2000dps)
#define BMI088_GYRO_BANDWIDTH_REG       0x10 // 带宽配置

在sensorsBmi088Bmp3xxAcquire中，调用其进行数据获取。最终数据在sensor.c中被管理。

// 读取陀螺仪原始数据 → 调用bmi088_gyro.c的底层实现
bmi088_get_gyro_data(&gyro_raw, &bmi088Dev);  
// 读取加速度计数据 → 调用bmi088_accel.c的底层实现
bmi088_get_accel_data(&accel_raw, &bmi088Dev); 
// 数据存入队列供状态估计器使用
xQueueOverwrite(gyroDataQueue, &gyro_raw);

初始化阶段关键寄存器操作

1. 芯片识别：读取0x00（BMI088_GYRO_CHIP_ID_REG）验证陀螺仪ID是否为0x0F
2. 模式配置：写0x11（BMI088_GYRO_LPM1_REG）设置NORMAL_MODE（0x00）
3. 量程设置：写0x0F（BMI088_GYRO_RANGE_REG）配置±2000dps（0x04）
4. 带宽配置：写0x10（BMI088_GYRO_BANDWIDTH_REG）设置ODR 100Hz（0x07）

数据采集阶段关键寄存器

1. 陀螺数据：突发读取0x02-0x07（BMI088_GYRO_X_LSB_REG）获取三轴16-bit数据
2. 加速度数据：读取0x12-0x17（BMI088_ACCEL_X_LSB_REG）获取三轴16-bit数据

2. 气压传感器BMP388（BMP3XX）

主要文件：

src/
├── drivers/
│   ├── bosch/
│   │   ├── interface/
│   │   │   ├── bmp3.h          // 气压计操作API声明
│   │   │   └── bmp3_defs.h     // 寄存器定义和配置宏（关键地址0x1B-0x31）
│   │   └── src/
│   │       └── bmp3.c          // 核心驱动实现
├── hal/
│   ├── src/
│   │   ├── sensors_bmi088_bmp3xx.c // 传感器集成层

寄存器表：


读操作时寄存器地址在bmp3_get_regs()和bmp3_set_regs()中实现。同样也是地址的最高位（第7位）用于标识读/写操作。
这里工程里面把BMI088和BPM388的部分通讯，通过共同函数来实现，容易混淆。
主要寄存器：

#define BMP3_PWR_CTRL_ADDR       0x1B  // 电源控制寄存器
#define BMP3_OSR_ADDR            0x1C  // 过采样配置寄存器
#define BMP3_CALIB_DATA_ADDR     0x31  // 校准数据起始地址
#define BMP3_DATA_ADDR           0x04  // 压力/温度数据寄存器

在bmp3_get_sensor_data中，实现数据获取。在sensors.c中，与BMI088传感器一起读取数据（sensorsBmi088Bmp3xxAcquire）。

 // 读取原始数据寄存器0x04-0x09
rslt = bmp3_get_regs(BMP3_DATA_ADDR, &reg_data[0], BMP3_P_T_DATA_LEN, dev);
// 使用0x31校准数据进行补偿计算
comp_press_temp(reg_data, data, dev);bmp3_get_sensor_data(BMP3_PRESS | BMP3_TEMP, &bmp_data);
baro->pressure = bmp_data.pressure; // 单位：Pa
baro->temperature = bmp_data.temperature; // 单位：°C

初始化阶段关键寄存器操作

1. 芯片识别：读取0x00（BMP3_CHIP_ID_ADDR）验证芯片ID是否为0x50（BMP388标识）
2. 校准加载：连续读取0x31-0x45（BMP3_CALIB_DATA_ADDR）获取21字节校准参数
3. 模式配置：写0x1B（BMP3_PWR_CTRL_ADDR）设置NORMAL_MODE（0x03）
4. 参数设置：写0x1C（BMP3_OSR_ADDR）配置过采样率（如压力x32，温度x1）

数据采集阶段关键寄存器

1. 状态检查：轮询0x03（BMP3_SENS_STATUS_REG_ADDR）的DRDY_STATUS位确认数据就绪
2. 数据读取：突发读取0x04-0x09（BMP3_DATA_ADDR）获取压力（3字节）和温度（3字节）原始数据

3. 光流PMW3901

寄存器表：

在函数pmw3901Init中实现了初始化，并未单独定义PMW3901的相关寄存器表，直接使用registerRead来进行寄存器的读取。
在InitRegisters中实现了寄存器初始化，向一系列寄存器写入特定值完成配置。
运动采集依靠pmw3901ReadMotion完成。
应用层在flowdeckTask中进行数据获取和调度。
上层集成在flowdeck_v1v2.c中。

4. ToF距离传感器VL53L1X

验证I2C通讯可以使用以下寄存器：

至于开发，则是使用VL53L1X API。
API相关函数在vl53l1_api等文件中。
相关API函数需要依赖ST官方的lib，位于lib文件夹下。
文件架构如下：

src/
├── drivers/
│   ├── src/
│   │   ├── vl53l1x.c          # 核心驱动实现
│   │   └── vl53l1x.h          # 接口定义
├── lib/
│   ├── vl53l1/
│   │   ├── core/              # ST官方驱动库

初始化时，通过动态分配I2C地址来操作不同的设备。

bool vl53l1xInit(VL53L1_Dev_t *pdev, I2C_Dev *I2Cx) {pdev->I2Cx = I2Cx;                          // 绑定I2C总线vl53l1xSetI2CAddress(pdev, newAddress);      // 动态分配I2C地址VL53L1_DataInit(pdev);                       // 加载官方预设参数VL53L1_StaticInit(pdev);                     // 静态校准初始化
}

上层集成在multiranger.c和zranger2_deck中，分别对应了五个方向（前后左右上）、下方向。
lib目录下对应的api函数，在开发时需要先将其内部修改为VL53L1X的相关驱动函数。
这里用到PCA95X4芯片来实现I2C接口扩展，将几个方向的TOF分成不同的地址。

传感器模块的应用

BMI088和BMP388（sensors）

对BMI088（六轴IMU）和BMP388（气压计）的功能实现主要集中在传感器初始化（sensorsBmi088Bmp3xxInit_I2C/SPI）、自检（sensorsBmi088Bmp3xxTest）、数据采集（sensorsBmi088Bmp3xxAcquire）以及传感器原始数据读取（readGyro/readAcc/readBaro等函数）。其中BMI088负责加速度计和陀螺仪数据，BMP388处理气压数据，两者通过sensors_bmi088_bmp3xx.c中的底层驱动实现硬件通信。

1. 在sensorsTask中，线程同步通过sensorsBmi088Bmp3xxDataAvailableCallback触发中断实现。通过sensorsAccelGet、sensorsGyroGet来获取加速度和陀螺仪数据，bmp3_get_sensor_data来获取气压计数据并传入队列中。
2. BMI088数据获取：通过axis3fSubSamplerAccumulate的传参，获取加速度计或陀螺仪数据，其对应的卡尔曼滤波器标志分别为MeasurementTypeAcceleration和MeasurementTypeGyroscope
3. BPM388数据获取：在函数sensorsReadBaro中，调用sensorsBmi088Bmp3xxReadBaro读取气压计数据，通过标志位MeasurementTypeBarometer传入到卡尔曼滤波器中kalmanCoreUpdateWithBaro
4. 加速度计和陀螺仪数据融合：通过函数kalmanCorePredict实现数据融合。
5. 陀螺仪数据与光流融合：在kalmanCoreUpdateWithFlow函数中实现

  float omegax_b = gyro->x * DEG_TO_RAD;float omegay_b = gyro->y * DEG_TO_RAD;

6. 气压计、光流、TOF高度补偿：rangeEnqueueDownRangeInEstimator()（来自zranger2.c）的高度数据与BMP388气压数据在heightKalmanUpdateByBaro()中融合

BMI088（IMU）与BMP388（气压计）的核心实现可分为三个层次：

1. 硬件接口层 (sensors_bmi088_spi.c)

• SPI初始化：spiInit()配置GPIO复用模式（SCK/MOSI为AF推挽输出，MISO为AF输入）
• DMA传输：spiDMATransaction()实现双缓冲DMA传输（spiTxBuffer/spiRxBuffer），10.5MHz时钟下完成加速度计+陀螺仪+气压计的全双工传输
• 中断同步：通过xSemaphoreGiveFromISR在DMA完成中断中释放信号量，保障1KHz控制周期的时序要求

2. 数据抽象层 (sensors.c)

• 多传感器调度：通过sensorsImplementation_t结构体统一调度不同传感器（如readGyro()实际调用bmi088ReadGyroSPI()）
• 校准接口：sensorsBmi088Bmp3xxAreCalibrated()实现出厂校准数据加载。
• 模式切换：setAccMode()动态调整加速度计量程（2g/4g/8g）

3. 控制应用层

• 数据消费：通过sensorsAcquire()将原始数据传递至stabilizerTask进行姿态解算
• 故障恢复：sensorsSuspend()/sensorsResume()在异常时禁用EXTI中断，防止数据冲突

4. 开发聚焦点

1. 修改SPI时序需同步调整spiConfigure()中的SPI_BaudRatePrescaler与DMA缓冲区大小
2. 新增传感器需在sensors.c中添加对应的sensorsImplementation_t实例

光流PMW3901（flowdeck_v1v2）

在deck中进行该设备的管理，并有两个不同的版本进行初始化和相关配置。
PMW3901光流传感器主要实现以下核心功能：

1. 数据采集与预处理

• 原始读取：通过pmw3901ReadMotion()获取原始光流数据（deltaX/deltaY）和传感器状态（squal/shutter）
• 坐标转换：在flowdeckTask()中执行坐标系翻转（accpx = -currentMotion.deltaY）适配飞行器安装方向
• 滤波处理：使用arm_mean_f32()实现移动平均滤波（USE_MA_SMOOTHING宏控制）

2. 测量值封装

• 数据结构化：构建flowMeasurement_t结构体（含dpixelx/dpixely/dt/stdDevX/stdDevY）
• 异常处理：通过OULIER_LIMIT阈值过滤异常值，outlierCount统计无效数据

3. 状态估计集成

• 数据注入：通过estimatorEnqueueFlow()将光流测量值送入卡尔曼滤波器并通过标志位MeasurementTypeFlow调用kalmanCoreUpdateWithFlow。在其中，与TOF、气压计都进行数据融合，并且也与陀螺仪共同来校准速度。
• 噪声建模：动态计算标准差（stdFlow=0.0007984f*shutter + 0.4335f）

4. 可配置性

• 参数调节：通过PARAM_ADD(PARAM_FLOAT, flowStdFixed)实现运行时噪声参数配置
• 模式切换：useFlowDisabled控制光流数据是否参与状态估计

5. 关键函数链

开始pmw3901Init() → flowdeckTask() → pmw3901ReadMotion() → 坐标转换 → 滤波处理 → estimatorEnqueueFlow()传入到卡尔曼滤波器

6. 硬件接口层 (flowdeck_v1v2.c)

• SPI通信：通过pmw3901ReadMotion()实现原始光流数据采集（deltaX/deltaY）
• 坐标系转换：将传感器原始坐标系转换为机体坐标系（accpx = -currentMotion.deltaY）
• 异常值过滤：基于OULIER_LIMIT阈值过滤异常光流数据（像素位移>100时丢弃）

7. 数据抽象层

• 数据平滑：提供移动平均(USE_MA_SMOOTHING)和低通滤波(USE_LP_FILTER)两种预处理方式
• 自适应噪声：useAdaptiveStd根据快门时间动态计算光流标准差（stdFlow=0.0007984f*shutter + 0.4335f）
• 测量封装：构建flowMeasurement_t结构体，包含时间戳(dt)、像素位移(dpixelx/dpixely)和噪声参数(stdDevX/Y)

8. 控制应用层

• 状态估计：通过estimatorEnqueueFlow()将光流数据注入卡尔曼滤波器，参与速度/位置估计
• 调试支持：通过LOG_GROUP(motion)实时记录光流原始数据和统计参数
• 参数配置：通过PARAM_GROUP(motion)实现运行时配置（禁用光流/切换自适应模式/设置固定噪声参数）

9. 开发聚焦点

1. 光流坐标系对齐（需与机体安装方向严格对应）
2. 噪声模型优化（修改stdFlow计算公式提升状态估计精度）
3. 预处理算法选择（MA/LP滤波的窗口大小与截止频率调参）
4. 多传感器融合（需与IMU、气压计数据在estimatorKalman.c中协同处理）

多距离VL53L1X （multiranger）

在deck中，该模块进行管理，首先调用mrInit及mrTest进行初始化，并开启mrTask线程，而后再进行测量等操作。
VL53L1X五方向测距功能主要通过以下核心函数链实现：

1. 传感器功能实现

• 初始化配置：通过mrInitSensor()调用vl53l1xInit()初始化5个方向传感器。（front/back/left/right/up），使用pca95x4SetOutput()控制XSHUT引脚唤醒对应传感器。
• 数据采集：mrTask()周期性调用mrGetMeasurementAndRestart()，获取五个方向的距离值，内部通过VL53L1_GetRangingMeasurementData()获取原始毫米级距离数据。
• 状态过滤：采用filterMask参数（默认1 << VL53L1_RANGESTATUS_RANGE_VALID）过滤无效测量值
• 数据发布：通过rangeSet()将处理后的米级数据存入全局变量（rangeFront/rangeUp等五个方向）。
• multiranger没有将数据传入到卡尔曼滤波器中（zrange2则通过kalmanCoreUpdateWithTof将数据流传入卡尔曼滤波器中），而是通过LOG_ADD_CORE函数上报到log中。

2. 应用场景

五方向距离数据通过rangeFront等全局变量在stabilizerTask中被调用，在sensorsAcquire中获取到更新的测量值，用于近地控制，通过PARAM_ADD(PARAM_UINT16, filterMask)可动态配置数据有效性判断规则。
在range.c中，通过LOG_ADD_CORE函数将六个方向的值都进行了log上报，原始工程中multiranger的几个方向数据做到这一步就截止了，并没有进行数据融合等操作。
如果要获取这些数据，可以通过logGetVarId(“range”, “up”)函数来操作，官方提供的示例中就有这么来操作的方式。

3. 硬件接口层

• 传感器初始化：通过mrInitSensor()依次初始化5个方向传感器（front/back/left/right/up），使用pca95x4SetOutput()控制XSHUT引脚唤醒传感器
• 数据采集：mrGetMeasurementAndRestart()循环调用VL53L1_GetRangingMeasurementData()获取原始毫米级距离数据

4. 数据处理层

• 状态过滤：通过filterMask参数（默认VL53L1_RANGESTATUS_RANGE_VALID）过滤无效测量值（如信号弱/超量程）
• 单位转换：将毫米转换为米（range/1000.0f）后存入全局变量（rangeFront/rangeUp等）

5. 控制应用层

• 数据发布：通过rangeSet()函数将处理后的距离数据注入系统测量池
• 避障支持：数据可直接被stabilizer等模块调用实现3D避障
• 调试接口：通过PARAM_GROUP_START(multiranger)动态配置过滤掩码

6. 关键函数链

开始mrInit() → mrTask() → mrGetMeasurementAndRestart() → VL53L1_GetRangingMeasurementData() → rangeSet() → LOG_ADD_CORE

7. 开发聚焦点

1. 传感器启动时序（XSHUT引脚控制间隔需≥2ms）
2. 测量频率优化（当前固定100ms周期，可调整M2T(100)参数）
3. 失效处理机制（当前无效数据返回32767）

8. 方向应用

通过定义不同的设备名称及其GPIO引脚，来分辨不同的方向。

下方向VL53L1X （zranger2_deck）

下方向测量需要更高的采样率（25ms vs 100ms）和独立的噪声模型，且作为光流/高度控制的核心传感器需避免多传感器地址冲突，所以只能单独拿出来。
下方向VL53L1X的实现主要通过以下核心函数链：

1. 功能实现

• 初始化配置：通过zRanger2Init()调用vl53l1xInit()初始化单向下测距传感器
• 数据采集：zRanger2Task()周期性调用zRanger2GetMeasurementAndRestart()，内部通过VL53L1_GetRangingMeasurementData()获取毫米级距离数据
• 噪声建模：采用指数噪声模型expCoeff = logf(expStdB/expStdA)/(expPointB-expPointA)动态计算高度测量标准差（multiranger没有这一种噪声建模方式，而是采用filterMask参数进行过滤）
• 数据发布：通过rangeSet(rangeDown, …)存储米级高度数据，并调用rangeEnqueueDownRangeInEstimator()注入卡尔曼滤波器

2. 与其他数据的融合

rangeDown数据流没有直接被其他模块调用，而是通过卡尔曼TOF更新，将数据流推入，间接被调用。

• rangeDown → 卡尔曼TOF更新 → 状态估计Z坐标 → 被各功能模块调用

3. 硬件接口层

通过zRanger2Init()调用vl53l1xInit()初始化传感器，配置I2C地址并设置中距离模式（VL53L1_DISTANCEMODE_MEDIUM）。与multiranger使用PCA95X4控制多传感器不同，这里采用直接I2C通信，确保25ms采样周期（M2T(25)）。

4. 数据处理层

在zRanger2GetMeasurementAndRestart()中获取原始数据后，执行：

• 异常过滤：丢弃>5m的测量值（RANGE_OUTLIER_LIMIT）
• 噪声建模：基于指数函数动态计算标准差（expCoeff*(distance-expPointA)）
• 单位转换：毫米转米（range_last/1000.0f）

5. 控制应用层

通过rangeEnqueueDownRangeInEstimator()将数据注入卡尔曼滤波器，在estimator_kalman.c中的kalmanCoreUpdateWithTof实现：

• 在kalmanCoreUpdateWithFlow()中结合光流数据解算速度
• 最终在kalmanCoreExternalizeState中进行控制体现

6. 开发聚焦点

1. 模式切换：中/长距模式在VL53L1_SetDistanceMode()的动态切换
2. 时序优化：25ms周期与光流10ms周期的数据同步
3. 失效恢复：扩展VL53L1_GetRangeStatus()状态解析实现自动校准
4. 多源融合：在kalmanCoreUpdateWithFlow()中结合光流数据解算速度
   在在kalmanCoreUpdateWithFlow()中，对this->S[KC_STATE_Z]进行了调用，后者在kalmanCoreUpdateWithTof()中对TOF数据进行了融合，最后通过kalmanCoreScalarUpdate两次更新。