MWCv2.2飞控算法LOOP()代码解析

"MWC代码解读，飞控算法解析" MWC（MultiWii Copter）是一种开源的多旋翼飞行控制器固件，用于无人机和其他小型飞行设备。本篇文章主要解读的是MWCv2.2版本中的`loop()`函数，它是整个飞行控制系统的主循环，负责处理实时的数据采集、计算和控制输出。 `loop()`函数是MWC固件的心脏，其运行频率通常设置为50Hz，即每20毫秒执行一次。在这个函数中，有以下几个关键部分： 1. **rcDelayCommand 和 trcSticks**: 这两个变量用于处理遥控器输入。`rcDelayCommand`追踪遥控器信号保持的时间，确保在执行命令（如启动或关闭电机）前，操纵杆必须保持在特定位置一段时间。`trcSticks`存储当前的操纵杆位置，用于组合成特定的控制指令。 2. **轴向处理（axis handling）**: `axis`变量用于遍历XYZ三个轴的陀螺仪和加速度计数据。这些数据对于计算飞行器的姿态至关重要。 3. **误差和偏差计算**: `error`和`errorAngle`表示实际姿态与目标姿态之间的差距，`delta`和`deltaSum`用于积分计算，累积误差以便进行PID控制。 4. **PID控制器**: PID（比例-积分-微分）控制器是飞行控制的核心。`PTerm`、`ITerm`和`DTerm`分别代表比例项、积分项和微分项。`PTermACC`和`PTermGYRO`分别对应加速度计和陀螺仪的PID输出，而`ITermACC`和`ITermGYRO`是对应的积分值。积分项有助于消除静态误差，比例项快速响应变化，微分项则帮助减少震荡。 5. **历史数据**: `lastGyro`数组存储上一次的陀螺仪读数，用于计算角速度增量。`delta1`和`delta2`用于计算双积分，以降低噪声影响。`errorGyroI`和`errorAngleI`存储了过去的误差，用于积分计算。 6. **时间戳管理**: `rcTime`记录上一次处理遥控信号的时间，确保每隔20毫秒更新一次。`timestamp_fixated`可能用于固定时间参考，确保在高精度时序下的稳定操作。 7. **遥控器数据读取**: 代码中注释了对两种特殊遥控器的支持：SPEKTRUM和OPENLRSv2MULTI。`readSpektrum()`和`Read_OpenLRS_RC()`函数分别用于读取这两种遥控器的数据，实现对遥控信号的解码和处理。 8. **循环滤波**: `computeRC()`函数执行循环滤波，它可能采用了平均滤波策略，将多个周期内的遥控信号值取平均，以滤除噪声，提高控制信号的稳定性。 9. **时间检查**: `currentTime > rcTime`判断确保每次循环都在正确的时序下执行。 MWCv2.2的`loop()`函数通过不断地迭代和调整，使得无人机能够根据遥控器输入实时调整飞行状态，同时通过PID控制实现稳定的飞行性能。这种细致的代码解读有助于理解飞行控制器的工作原理，对于开发和优化飞行控制固件具有重要意义。