Pixhawk原生固件PX4之offboard

offboard

PX4中的offboard(暂译作外部控制)是一个非常强大的功能。可以接受来自外部的控制指令，按照目前的了解来看，offboard搭配上MAVROS以及类似于TX1/NUC板载计算器，在PX4这个稳定的飞控平台上加上视觉处理或者人工智能，大有可为。

配置说明

官网上有一个外部控制的例程，让飞机起飞到2米的高度(x = 0, y = 0, z =0)，这里使用的是ROS里面的坐标系，并不是PX4固件中的NED，MAVROS中已经完成了坐标转换，这一点的话暂时不用深究。

本文主要讲一讲使用数传以及Pixhawk飞控板的外部控制方法：所需的只是一台Ubuntu系统的电脑以及一架带有数传的飞机。首先还是环境的配置，这一篇博文已经讲的非常清楚了，不知道为什么就是想装ROS indigo + Gazebo7。按教程offboard仿真通过以后就可以上真机测试了。

需要做的只有三个步骤：

1. 配置Pixhawk上的Telema 2 修改SYS_COMPANION参数改成Companion Link(57600)
   
2. 将飞控连上MAVROS
   
roslaunch mavros px4.launch fcu_url:=serial:///dev/ttyUSB0:57600


3. 运行外部控制例程

   rosrun offb_ctrl offb_ctrl_node

   搞定 起飞了

   • 其中offboard的C++例程如下

/**
 * @file offb_node.cpp
 * @brief offboard example node, written with mavros version 0.14.2, px4 flight
 * stack and tested in Gazebo SITL
 */

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>

mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "offb_node"); //节点重命名为offb_node 供ROS调用
    ros::NodeHandle nh; // 打开roscpp节点

    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("mavros/state", 10, state_cb);// 订阅函数的参数1表示主题  参数2表示缓存的消息队列长度  消息3表示消息更新时调用的回调函数
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped> // 公告发布者的主题，参数2表示输出到订阅者的最大消息队列长度
            ("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"); // 创建一个客户端来设置模式

    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(20.0);

    // wait for FCU connection
    while(ros::ok() && current_state.connected){
        ros::spinOnce();
        rate.sleep();
    }

    geometry_msgs::PoseStamped pose;
    pose.pose.position.x = 2;
    pose.pose.position.y = 2;
    pose.pose.position.z = 2;

    //send a few setpoints before starting
    for(int i = 100; ros::ok() && i > 0; --i){
        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep(); // 保证更新频率为20Hz，自动调整睡眠时间
    }

    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD"; // 调用者请求

    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    ros::Time last_request = ros::Time::now();

    while(ros::ok()){
        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.success){ // 被调用者响应
                ROS_INFO("Offboard 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();
            }
        }

        local_pos_pub.publish(pose);

        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}

• 关于ROS坐标系与PX4坐标系的转换

如图所示：iris四旋翼蓝色桨对应的为机头方向。
ROS中的坐标是ENU(东北天)坐标系，而PX4中是NED(北东地)坐标系。MAVROS这个ROS包中已经进行了坐标转换。

通过ROS节点控制PX4飞机的位置时，坐标有如下的对应关系

ROS	PX4
x	y
y	x
z	alt(-z)

进行控制时要把ROS坐标系跟PX4的坐标系分离开来，摄像头坐标系Z轴沿光轴向下，X轴朝机头方向，根据右手法则确定Y轴指向飞机右侧。使用AprilTag或ArUco库可以直接通过相应编码的二维码得到飞机的位置姿态。对于位置误差

err_x = pose.x;
err_y = -pose.y;
err_z = -pose.z;

根据位置误差可以得到速度的设定值

• 目前存在的问题：

  1. 没有GPS或者辅助定位装置，这种直接设置位置的飞行是如何办到的？
  2. MAVROS运行时，offboard模式无法打断，遥控器都切不过去，危险
  3. ROS都不会，哪么搞？

  问题2的话直接采用手动设定模式的方法即可

奇技淫巧

Nomachine是一款高级的远程桌面访问工具。
怎么说呢，机载协同计算机（跑着Ubuntu系统）上装一个这个软件作为目的主机，然后本地计算机（Windows或者Ubuntu系统的电脑）上也装上这个软件，只要两台计算机在连在同一个局域网中，就能在本地操作飞机上的计算机了。
直接远程控制飞机上的电脑，QQ远程协助用过吧

就像上面这样，在win7上可以操作另一台Ubuntu主机，这时你就可以进Ubuntu的Terminal跑你想运行的ROS节点了。

而且操作也是非常简单

1. 打开软件
   

   
   可以看到这里已经搜到一个网内的客户机了

2. 点击连接(Connect)
   

   
   这里我是用Win7控制Ubuntu，因此在这里需要输入目标主机Ubuntu的账号密码，登陆后就可以看到上面图中显示的Ubuntu界面了。

3. 操作说明
   

   
   就但是远程协助那样去操作就行了。
   这里要介绍的是退出操作，从客户机退出需要将鼠标移动到画面的右上角，点下去你就知道了。

软件面向用户的体验做的不错，人人都能直接上手，赞一个！

TroubleShooting

• GeographicLib报错

  [FATAL] [1506043767.570923014]: UAS: GeographicLib exception: File not readable /usr/share/GeographicLib/geoids/egm96-5.pgm | Run install_geographiclib_dataset.sh script in order to install Geoid Model dataset!
  [mavros-1] process has died [pid 14648, exit code -5, cmd /home/donghee/catkin_ws/devel/lib/mavros/mavros_node __name:=mavros __log:=/home/donghee/.ros/log/a61f72a6-9f34-11e7-80c3-7cb0c21790bc/mavros-1.log].
  log file: /home/donghee/.ros/log/a61f72a6-9f34-11e7-80c3-7cb0c21790bc/mavros-1*.log

  解决方案
  到catkin空间中下载mavros源码，并运行其中的脚本，应该是解决依赖性问题。

  cd ～/catkin_ws/src/mavros/mavros/scripts
  sudo ./install_geographiclib_dataset.sh