目的是在model中写一个与mc_att_control类似的model加入自己的姿态控制算法并替换之
姿态控制算法simlink搭建生成并简单说明
PX4 v1.9.2姿态控制接口简单介绍
SITL以及HITL简单调试方法
我们设计了一个简单的串级pid控制器，如下图
输入信号：飞机的当前姿态角以及当前角速度（state）、期望姿态（reference）、命令（command包括复位和模式选择）
输出信号：为归一化的x、y、z的控制信号范围为[-1,1]要和PX4姿态控制输出接口吻合
中间过程就是一些外环，内环控制了，这里就不详解了。
然后我们用simlink中代码生成工具bulid并配置一些必要的参数，就可以生成我们需要的嵌入式C++代码了，接下来就是要在飞控板中实际的验证我们的算法。
关于如何在PX4中添加模块，你可以参考官网的例程(hello_sky)。
根据上一节可知我们的控制器至少需要三种接口，分别为当前姿态接口，期望姿态接口以及输出接口。大致这样：
其中 当前的控制模式 需要飞机的是否解锁信息为的时判断是否启动控制器和是否reset，还有飞机当前的飞行模式，这里我们主要判断的是飞机是否处在STABILIZED模式，其他的模式控制不在我们的讨论之中。
这里我先介绍上述接口对应的TOPIC:
期望的角度：manual_control_setpoint_s
当前的角度以及角速度 ：vehicle_attitude_s
当前的控制模式 ：vehicle_control_mode_s
控制输出 ：actuator_controls_s
注意当飞机处在STABILIZED模式时，遥控器的俯仰、翻滚、油门、偏航通道分别对应manual_control_setpoint_s结构体中的x、y、z、r。 这四个量都是归一化后的数据范围为[-1,1]，其中的x、y、z可以直接送给我们的mc_att_linear_pid模型，而偏航r需要转换一下，否则当你控制yaw方向时，飞机yaw方向会随着遥控器的偏航杆回中飞机的yaw也会回到初始位置。
在mc_att_control_main.cpp中这个部分的对应的函数是MulticopterAttitudeControl::generate_attitude_setpoint()
主要的功能是把manual_control_setpoint_s结构体中的x、y、z、r，转换成我们需要的期望角度。
下图是源码mc_att_control_main.cpp中STABILIZED模式下简单的流程图（只跟踪了stabilized模式）。
我们在调试代码的时候主要用到gazebo软件在环仿真，以及gazebo硬件在环仿真。
用到的函数是 **PX4_INFO()**可以在终端中打印信息，用于调试，
初期可以软件在环仿真，确保程序架构正确，能够正常运行。后期可以gazebo硬件在环仿真加入遥控器在连接PX4飞控实时的查看模型的运动情况
在运行仿真前要把自己的模块加入到sitl 中的defult.cmake如下
SITL
终端输入
make px4_sitl_default gazebo
然后终端中就会实时打印你在模块中写的**PX4_INFO()**内容了
HITL
HITL中，查看调试信息PX4_INFO() 需要在qgc地面站中查看，前提是你的PX4已成功连接gazbeo，qgc成功连接gazebo。具体配置可参考官网HITL Simulation.
key:当你的model处在运行状态，但是**PX4_INFO()**信息却没有打印出来，你可已重启一下这个model试一下。