使用mavlink收ref和imu
可以选择发出pwm和torque;发出torque的话，没有考虑pwm的饱和;发出pwm的话，电机的参数也不准。
参考GAAS
写收的imu,gps，姿态三个接口，并在程序中可以切换，最好可以用遥控器控制HIL mode开关;
写发的torque，pwm的接口
打包imu，gps用mavlink发送;
解包torque发给gazebo;
真值和pva ref已有现成程序。
注意坐标系转换！
目标:
飞控端：参照官方教程
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的控制器，正在开发中。
首先感谢Shupeng哥的详细讲解~完全讲通了mavlink, mavros，px4之间的通信链路。下面根据我们的目标： 自定义168飞控和电脑（仿真器）之间的通信，记录下分享会内容。
视频地址
mavlink官网上，可以看到一个xml，里面定义的是PX4的message。
对于168，我们也有类似xml。文件地址
注意，因为我们用的是Protocal 1.0， 所以msg id最好在256以内。
我们可以找一个现在飞控中不用的msg id，比如
然后把名字，data type之类的都修改掉，填上我们想要的消息格式。这里，我们定义一个类似ROS中的wrench格式消息，内容是6个float：[force x, force y, force z, torque x, torque y, torque z]。
之后，可以根据这个网站mavlink generator，把xml转换成mavlink，就会得到mavlink/v1.0/,里面是一堆header。在header中的pixhawk文件夹，可以根据名称找到 mavlink_msg_xxx.h，里面有我们需要用的函数。
之后，可以把mavlink文件替换\mavros\libmavconn\include\mavlink。下面我们转战mavros!
启动mavros会启动什么？我们先进入mavros.cpp看一下。这里定义了两个比较重要的link，一个是gcs_link，和地面站通信所用，一个是fcu_link，和飞控通信所用。
在serial.cpp中，代码在收到msg之后，就会进入两个callback函数类似的东西：一个是把raw msg直接变成ros msg发出去，这个一般不用；另一个是进入plugin_route_cb，这个就会根据msg id来trigger对应的plugin。
所以我们要新注册plugin，来处理这些msg。大概分三步：
写plugin源文件
比如这里以GPS为例，注意头文件和尾部：
之后，需要根据message_id定义message_handler.
我们目的是接收wrench数据，然后用rostopic发出来。
修改Cmakelists.txt
修改mavros_plugins.xml
这里需要jiahao补充下，可参照PX4 GUIDE
在hector_quadrotor里，空气动力学的计算在这里
具体作用在link上，这是gazebo中机体现在的状况，我们可以用force和wrench改变他们：
从controller那里收到wrench后，直接可以用下面两个API传给模型。这两个函数应该是gazebo内置的。
初步推断，sitl_gazebo的动力学解算在这里：
主要过程是在update()函数中，算出force和moment之后，调用gazebo API,对环境中的刚体施加力和力矩：
后面的dynamics应该需要查看gazebo源代码，比如我们看关于link的dynamics,可以发现每一种solver都对应一种link。
根据不同的机械结构，比如关节，铰链等等，有不同的dynamics，这里四旋翼可以就简化成link计算。
最后，运行的时候plugin怎么调用的？简单来说，是被编译成了一个.so文件，然后我们使用.sdf文件来找到这个.so文件，就可以调用其作为一个标准的ros插件。具体可以参考gazebo
在实验室服务器上配置，出了很多问题
gazebo需要升级到9.9以上https://answers.gazebosim.org//question/24645/how-to-update-gazebo-900-to-latest-after-ros-melodic-full-desktop-install/
ignition_math库需要升级
总之就是没有按文档。。正确做法一定不能安装ros desktop-full,只能安装desktop，之后再安装gazebo！
mavros中的PositionTarget类型，和common_msg一样，甚至更多
https://github.com/mavlink/mavros/blob/7a883b7ab6bcb6f2845c186998fa622d9ee1609c/mavros/src/plugins/setpoint_raw.cpp
连上motion planning的流程是，首先要arm t
cd ~/XTDrone/control/
python multirotor_keyboard_control.py iris 1 vel
也就是有/mavros/cmd/arming service
然后关掉这个py，开始运动规划发送PositionTarget 到/mavros/setpoint_raw/local
文件地址
在Load()里读取了sdf文件，并且定义了一堆virtual sensors callback
如果没有enable serial,那么就会配置tcp udp
对应的，sdf应该修改为
然后看看update函数
这里好像是把QGC信息传到串口？
这儿把活都干完了。可以看到，不光把sensor msg传到了px4,还把真值也传过去了。
看看handle_actuaor_control()
根据是否arm，来设定input ref
然后这个函数应该就是从飞控接收HIL_ACTUATOR的。
mavlink_interface.cpp中找到这个函数：（xtdrone中的版本不是master,所以写在了一起）
那么看看handle_control()。这里根据input_reference，用了一个pid来控制gazebo中的joint(为啥不是link?)
现在的问题是，
