HITL todo
使用mavlink收ref和imu
可以选择发出pwm和torque;发出torque的话，没有考虑pwm的饱和;发出pwm的话，电机的参数也不准。
FC
写收的imu,gps，姿态三个接口，并在程序中可以切换，最好可以用遥控器控制HIL mode开关;
写发的torque，pwm的接口
mavros
打包imu，gps用mavlink发送;
解包torque发给gazebo;
真值和pva ref已有现成程序。
注意坐标系转换！
168通信开发流程
目标:
- 168发送torque (3 linear, 3 angular)到CPC，CPC使用MAVROS接收信息再转发到gazebo。
- CPC将gazebo的sensor msg 打包发送给168.
因为第一个目标涉及了很多方面，所以比较困难，下面对此进行步骤分解。
飞控端：参照官方教程
CPC端：
-
在/opt/ros/kinetic/include/mavlink/v1.0/pixhawk 中新建 mavlink_msg_xxx.h， 根据PX4确定 MAVLINK_MSG_ID_XXX ;
-
可以仿照mavlink_msg_local_position_ned.h编写对应函数，比如编写mavlink_msg_xxx_decode() 函数，消息解包后，给ros msg赋值;
-
在mavros中编写对应plugin，主要是定义handler:
gazebo端：
目前还没有直接用torque的控制器，正在开发中。