168-HITL-dev-manual

HITL todo

使用mavlink收ref和imu
可以选择发出pwm和torque;发出torque的话，没有考虑pwm的饱和;发出pwm的话，电机的参数也不准。
参考GAAS

FC

写收的imu,gps，姿态三个接口，并在程序中可以切换，最好可以用遥控器控制HIL mode开关;
写发的torque，pwm的接口

mavros

打包imu，gps用mavlink发送;
解包torque发给gazebo;
真值和pva ref已有现成程序。
注意坐标系转换！

168通信开发流程

目标:

1. 168发送torque (3 linear, 3 angular)到CPC，CPC使用MAVROS接收信息再转发到gazebo。
2. 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的控制器，正在开发中。

168通信开发详细版

首先感谢Shupeng哥的详细讲解~完全讲通了mavlink, mavros，px4之间的通信链路。下面根据我们的目标： 自定义168飞控和电脑（仿真器）之间的通信，记录下分享会内容。
视频地址

• 太长不看版：
  修改xml–>mavlink generator生成header–>header放到px4和/opt/ros/kinetic/include/mavlink 中（覆盖）–> 编写mavros plugin–>编写飞控上的uORB handler。

目前进度

1. 仿真器wrench controller开发完成，可以直接发送力和力矩控制飞机。
2. 仿真器可以发送virtual imu 到mavros，但是否飞控成功接收，还有待测试。
   请大家在这里下载已开发版本，bitbucket
   这里我们需要解决的是，新定义一个类似ROS中的wrench格式消息，内容是6个float：[force x, force y, force z, torque x, torque y, torque z]，从PX4计算后传到mavros中，再传给仿真器的 wrench controller. 根据实际情况，这里的力都是在body frame下。

step 1：自动生成mavlink

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!

step 2：注册plugin

启动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
  

step 3：修改uORB

这里需要jiahao补充下，可参照PX4 GUIDE

gazebo models

在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

XTDRONE环境配置

在实验室服务器上配置，出了很多问题
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！

xtdrone–cpc联调

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

HITL gazebo

文件地址
在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?)

现在的问题是，

1. hitl没有办法进入offboard，也连不上mavros
2. sitl的set positiontarget不能用，要用enu…
   todo
3. 多机话题匹配,communication挂了
4. motion 坐标转换？
   coordinate frame