再给个飞控方面的建议，当前main.c中东西太多了，我的建议是用一个新的文件承载飞行的状态机，然后main.c只调用接口即可。当前stm32f103加逻辑绰绰有余，如果后续要进一步的话，建议改为f4甚至h7系列，然后用上freeRTOS。还有HAL库也可以了解一下，是非常方便的，毕竟后续就要把更多精力放在算法而不是硬件配置上。

还有mpu6050，如果只用加速度夹角判断欧拉角只适用于静态，动态下会有问题，如果不加相关算法的话精度会低。还有这里yaw和pitch的定义似乎和常见的不同，自己写飞控的话，我还是建议判断逻辑用四元数，毕竟不存在奇点，还不会像当前这样判断区间是四棱锥而不是圆锥。欧拉角主要传回来给人看。不过具体要改到什么程度就要看你的经济与时间预算了，当前还是工作得很好的。