X小四轴串级PID调节振荡

我将小四轴的两端用绳子固定在椅子上，调节俯仰内环PID，可是我仅调节内环P参数时，四轴一开始总是往前倒，总是后面两个电机的力度比前两个电机的力度大，加入D后可以在平衡位置维持一会，然后产生等幅振荡，震荡幅度越来越大最大45°左右，源程序是借鉴别人的十字四周，我将MPU6050的方向改了一下，静态是姿态角偏差1~2°左右，希望大家帮我找下原因，谢谢！附上源程序

四轴-四元数-互补滤波-串级PID -STCunio的电机和桨.rar (177.64 KB )