# Mavros在线控制 (C++)

**注意：本文适用于所有Kerloud无人机产品**

mavros ROS程序包可视为运行ROS的计算机、启用MAVLink的无人机和启用MAVLink的地面站三者间的网关，其安装流程可参考： <https://dev.px4.io/v1.9.0/en/ros/mavros_installation.html> 或 <https://github.com/mavlink/mavros/tree/master/mavros#installation>. 我们提供了一个现成的工程环境，用户可以在如下目录中开发其应用程序：\~/catkinws\_open，该目录下包含三个文件夹：mavlink、mavros和off\_mission。由于国内网络限制，如果用户想从头开始操作，可能会在尝试访问github资源或 rosdep时出现中断。

{% hint style="info" %}
鉴于持续的软件更新，工作目录逐步迁移到: \~/src/uav\_workspace/catkinws\_offboard
{% endhint %}

## Pixhawk设置

为了使能Pixhawk的offboard控制模式，建议用户参照如下网页中的介绍： <https://dev.px4.io/master/en/ros/offboard_control.html>, <https://dev.px4.io/master/en/companion_computer/pixhawk_companion.html>。 在我们的环境下，用户可以按如下内容对Pixhawk板载参数进行设置：

* 针对默认安装的[Kerloud mini](http://cloudkernel-tech.gitee.io/kerloud_mini/en/)自驾仪:

  ```
    MAV_0_CONFIG = TELEM 1
    MAV_0_MODE = Normal
    MAV_0_FORWARD = Enable
    SER_TEL1_BAUD = 57600

    MAV_1_CONFIG = TELEM 2
    MAV_1_MODE = Onboard
    MAV_1_FORWARD = Enable
    SER_TEL2_BAUD = 921600
  ```

## 如何编译

编译环节我们采用catkin-tools，它比常规的catkin\_make快很多。 有关catkin工具的详细信息，请参见：<https://catkin-tools.readthedocs.io>。 用户可以按以下指令对工程环境进行编译：

```
    cd ~/catkinws_open
    catkin init  # initialize the workspace
    catkin build -j2  # build the workspace with two threads only to avoid overloading in Jetson Nano
```

编译过程耗时超过十分钟，首次操作请耐心等待。使用如下指令可清除工程环境：

```
    cd ~/catkinws_open
    catkin clean
```

## 例程代码讲解

off\_mission文件夹下的例程实现航点飞行任务。航点信息在yaml文件中定义，其路径为src/ off\_mission/launch/waypoints\_xyzy.yaml，文件中还包含ENU坐标和相应的偏航值。该软件包支持SITL（软件在环）仿真，用户可以在实际飞行前对自身软件进行测试。 为了描述清晰，我们按顺序对main()函数进行讲解。

（1） 对于订阅、发布和服务信息的声明

```
    //subscriptions, publications and services
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("mavros/state", 10, state_cb);
    ros::Subscriber local_pose_sub = nh.subscribe<geometry_msgs::PoseStamped>
            ("mavros/local_position/pose", 10, local_pose_cb);
    ros::Subscriber rc_input_sub = nh.subscribe<mavros_msgs::RCIn>
            ("mavros/rc/in", 5, rc_input_cb);
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
            ("mavros/setpoint_position/local", 10);
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
            ("mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
            ("mavros/set_mode");
```

我们通常会在main程序的开头对ROS订阅、发布和服务信息进行声明。 mavros软件包中的各类主题、服务易于使用，可用于查询状态信息、发送指令等，细节详见：<http://wiki.ros.org/mavros>。例如，若要获取飞机的本地位置信息， 则需要订阅mavros/setpoint\_position/local主题。需注意，采用ENU（East-North-Up，东北上）坐标系。

(2) 载入航点信息，检查仿真结果

```
    // load yaml files
    XmlRpc::XmlRpcValue wp_list;
    private_nh.getParam("waypoints",wp_list);

    // simulation flag
    int simulation_flag = 0;
    private_nh.param("simulation_flag", simulation_flag, 0);
```

正常通过后，航点数据将从参数管理器中进行加载，并初始化为航点目标。

(3) 在SITL仿真中解锁飞机

```
    if (simulation_flag == 1)
    {
        //set offboard mode, then arm the vehicle
        if( current_state.mode != "OFFBOARD" &&
            (ros::Time::now() - last_request > ros::Duration(5.0))){
            if( set_mode_client.call(offb_set_mode) &&
                offb_set_mode.response.mode_sent){
                ROS_INFO("Offboard mode enabled");
            }
            last_request = ros::Time::now();
        } else {
            if( !current_state.armed &&
                (ros::Time::now() - last_request > ros::Duration(5.0))){
                if( arming_client.call(arm_cmd) &&
                    arm_cmd.response.success){
                    ROS_INFO("Vehicle armed");
                }
                last_request = ros::Time::now();
            }
        }
    }
```

实际飞行中，我们只需要解锁飞机、切换到OFFBOARD模式即可开启任务。在SITL仿真中，我们需使用mavros/cmd/arming服务来解锁飞机。

（4） 发布当前航点

```
    pose = waypoints.at(current_wpindex);
    local_pos_pub.publish(pose);
```

程序将通过mavros主题将当前航点坐标发布到无人机。请注意，该主题必须在OFFBOARD模式下一直发布，否则Pixhawk将启动故障安全机制、关闭OFFBOARD模式。

（5） 降落后锁定

```
    if( arming_client.call(disarm_cmd) &&
        arm_cmd.response.success){
        ROS_INFO("Vehicle disarmed");
    }
```

到达最终航点并成功降落后，程序将调用mavros/cmd/arming服务自动对飞机进行锁定。

## 如何使用

### 软件在环仿真

软件在环仿真（Software In The Loop）需要和PX4固件一起运行，我们推荐用户使用我们维护的版本<https://gitee.com/cloudkernel-tech/Firmware>，以确保和飞机软件兼容性。

因为依赖特定的软件环境，**软件在环仿真目前只支持在安装ubuntu 18.04的PC电脑中运行，而不能直接在机载电脑中运行**。详细设置软件环境的说明可以参考<http://cloudkernel-tech.gitee.io/kerloud_mini/en/ADVANCEDDEV.html>。 用户还需要把位于\~/catkinws\_open/的整个offmision control软件包迁移到PC中，

假设PX4固件在\~/src/Firmware位置，那么基于gazebo的软件在环仿真可以按以下指令启动：

```
    cd ~/src/Firmware
    make px4_sitl_default gazebo
```

然后通过如下指令启动offboard控制飞机：

```
    cd ~/catkinws_open
    source devel/setup.bash
    roslaunch launch/wpmission_sitl.launch
```

![](https://872799980-files.gitbook.io/~/files/v0/b/gitbook-x-prod.appspot.com/o/spaces%2F-Ml3VlSesh_3iSoHlURD%2Fuploads%2Fgit-blob-092193e3470d3319ff8db10b43b92b0985ca8671%2FROS_sitlview.png?alt=media)

### 实际飞行

实际飞行中，需严格按如下顺序操作以避免出现未知异常：

* 将带有接口的电池可靠安装到飞机，确保飞行中不会脱落；
* 确保遥控器所有开关均处于初始位置，且油门降到最低；
* 电池接通上电；
* 等待GPS锁星：通常可在2-3分钟后听到提示音，且pixhawk主灯变绿；
* 确保飞机出于正常待机模式，且飞机周围无人；
* 按下安全按钮（保持1S）预解锁无人机，按钮指示灯将变为双闪；
* 机载电脑启动需等待几分钟，通过本地wifi网络进行登录，确认路径\~/catkinws\_open/src/off\_mission/launch/waypoints\_xyzy.yaml下的 航行任务，运行如下指令启动offboard控制模块：

  ```
    # set read/write privilege for the USB-ttl port
    sudo chmod a+rw /dev/ttyPixhawk

    cd ~/catkinws_open
    source devel/setup.bash
    roslaunch launch/wpmission.launch
  ```

  **提示:** 机载电脑也可以在启动期间自动执行offboard任务，用户可将工程目录下的 startup.sh作为参考模板。
* 同时拉低油门、右推偏航摇杆解锁无人机，解锁成功将听到一次长音提示；
* 使用指定的摇杆（如通道7）切换到OFFBOARD模式，然后无人机将自动开始航点飞行任务。恭喜飞行成功！