我用simulink控制四旋翼飞行，但是一飞起来就失控

用stm32做的四旋翼，飞控没有写在stm32里面，而是用simulink完成的，stm32只做数据传递的作用。
我做了实验，一起飞就往一边偏，姿态指令根本没用，我不知道是不是我的控制模型出了问题，有没有大佬帮忙看一下？

我的想法是用游戏手柄输入期望角度，比如俯仰角5度，然后我的四旋翼就能跟着俯仰5度，用的是PI控制。我看网上的模型是手柄输入角速度的双环控制，外环控制角速度，内环控制角度，我是不是也该这么干？

这是那个大模块的内部结构，功能就是将角度差转化为电机的控制量通过串口输出给stm32，再通过串口获得四旋翼传回的姿态数据。

这就是前面的控制器，负责将角度差转化为电机的控制量。

这是4个转化程序中的其中一个，就是电机转速跟随经过PID调节后的角度差线性变化，就两行，非常简单粗暴。
我的问题是：
1、我的PID控制模型是否正确？能不能用手柄直接控制角度然后通过PID调节？
2、我的控制器是不是写的不好需要修改？我得承认这真的是我凭感觉写的，我不知道该怎么写更好。
不知道有没有大佬解决一下我的问题，我觉得我卡在这里了，真的只能问了。