PX4飞控接收机载电脑的自定义Mavlink消息

前言

 PX4飞控接收机载电脑Nvidia Jetson的自定义Mavlink消息，将机载电脑获取的数据发送给飞控，本篇实现的是已知路径文件中的数据发送与接受，数据格式TUM格式，发送其中的x、y、z三轴位置数据，后续可考虑将机载电脑实时解算的数据发送给PX4飞控。

一、PX4添加自定义uorb和mavlink消息

1.在飞控端接收机载电脑传来的数据，需要定义uorb消息来接收。

  在下图位置新建  key_command.msg：

uint64 timestamp			# time since system start (microseconds)

float32[3] f_a

 2.修改下面路径下的common.xml文件

 在下图位置添加

<message id="229" name="KEY_COMMAND">
     <description>Keyboard char command.</description>
     <field type="uint64_t" name="time_usec" units="us">Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.</field>
     <field type="float[3]" name="f_a">float32_array</field>
   </message>

修改下图 CMakeLists.txt 文件

 在下图位置添加：

key_command.msg

3.使用MAVLink的图形用户界面代码生成器mavgenerate.py生成MAVLink库文件：

python3 -m mavgenerate

然后在打开的图形界面中执行下面的操作:

XML：~/Firmware/mavlink/include/mavlink/v2.0/message_definitions/common.xml
OUT：~/Firmware/mavlink/include/mavlink/v2.0
Language：C
Protocol：2.0
Validate：勾选
点击Generate按钮，在~/Firmware/mavlink/include/mavlink/v2.0/中会生成common和standard文件夹。

二、PX4接收自定义mavlink消息

1.修改下图 ​​mavlink_receiver.h​

 下图位置添加：

#include <uORB/topics/key_command.h>

 下图位置添加：

void handle_message_key_command(mavlink_message_t *msg);

 

 下图位置添加：

orb_advert_t _key_command_pub{nullptr};

 2.修改下图​​ mavlink_receiver.cpp​ 文件

 下图位置添加：

case MAVLINK_MSG_ID_KEY_COMMAND:
        handle_message_key_command(msg);
        break;

 

 下图位置添加：

void MavlinkReceiver::handle_message_key_command(mavlink_message_t *msg)
{
    mavlink_key_command_t man;
    mavlink_msg_key_command_decode(msg, &man);
    struct key_command_s data = {};

    data.timestamp = hrt_absolute_time();
    data.f_a[0] = man.f_a[0];
    data.f_a[1] = man.f_a[1];
    data.f_a[2] = man.f_a[2];

    if (_key_command_pub == nullptr)
    {
        _key_command_pub = orb_advertise(ORB_ID(key_command), &data);
    }
    else
    {
        orb_publish(ORB_ID(key_command), _key_command_pub, &data);
    }
}

三、PX4打印自定义mavlink数据

 1.在下图目录新建 receive_data 文件夹 

在该文件夹下创建 CMakeLists.txt 文件和 receive_data.cpp 文件 

 在CMakeLists.txt文件中写入:

px4_add_module(
    MODULE modules__receive_data
    MAIN receive_data
    COMPILE_FLAGS
    SRCS
        receive_data.cpp
    DEPENDS
    	px4_work_queue	
    )

 在receive_data.cpp文件中写入:

#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/posix.h>
#include <unistd.h>
#include <stdio.h>
#include <poll.h>
#include <string.h>
#include <math.h>

#include <uORB/uORB.h>
#include <uORB/topics/key_command.h>

extern "C" __EXPORT int receive_data_main(int argc, char **argv);

int receive_data_main(int argc, char **argv)
{
    int data_sub_fd = orb_subscribe(ORB_ID(key_command));
    orb_set_interval(data_sub_fd, 1000); // limit the update rate to 1000ms

    px4_pollfd_struct_t fds[1];
    fds[0].fd = data_sub_fd, fds[0].events = POLLIN;

    int error_counter = 0;

    while (true)
    {
        int poll_ret = px4_poll(fds, 1, 1000);

        if (poll_ret == 0)
        {
            PX4_ERR("Got no data within a second");
        }

        else if (poll_ret < 0)
        {
            if (error_counter < 10 || error_counter % 50 == 0)
            {
                PX4_ERR("ERROR return value from poll(): %d", poll_ret);
            }

            error_counter++;
        }

        else
        {
            if (fds[0].revents & POLLIN)
            {
                struct key_command_s input;
                orb_copy(ORB_ID(key_command), data_sub_fd, &input);
                PX4_INFO("Recieved data x,y,z : %0.5f %0.5f %0.5f", (double)input.f_a[0], (double)input.f_a[1], (double)input.f_a[2]);
            }
        }
    }
    return 0;
}

2.把该模块添加到系统的配置文件default.cmake中

 在下图位置添加：

receive_data

 

3.编译Firmware

 刷新固件，用USB连上飞控板，在Firmware中执行:

make px4_fmu-v5_default upload

四、MAVROS发送自定义mavlink数据

1.新建 keyboard_command.cpp ​​​文件夹

 keyboard_command.cpp中代码如下：

#include <mavros/mavros_plugin.h>
#include <pluginlib/class_list_macros.h>
#include <iostream>
#include <std_msgs/Char.h>
#include <mavros_msgs/keyboard_command.h>

namespace mavros
{
    namespace extra_plugins
    {
        class KeyboardCommandPlugin : public plugin::PluginBase
        {
        public:
            KeyboardCommandPlugin() : PluginBase(),
                                      nh("~keyboard_command")

            {};

            void initialize(UAS &uas_)
            {
                PluginBase::initialize(uas_);
                keyboard_sub = nh.subscribe("keyboard_sub", 10, &KeyboardCommandPlugin::keyboard_cb, this);
            };

            Subscriptions get_subscriptions()
            {
                return {/* RX disabled */};
            }

        private:
            ros::NodeHandle nh;
            ros::Subscriber keyboard_sub;

            void keyboard_cb(const mavros_msgs::keyboard_command::ConstPtr &req)
            {
                //std::cout << "Got Char : " << req->data << std::endl;
                mavlink::key_command::msg::KEY_COMMAND key_command{};
                key_command.f_a[0] = req->f_a[0];
                key_command.f_a[1] = req->f_a[1];
                key_command.f_a[2] = req->f_a[2];
                UAS_FCU(m_uas)->send_message_ignore_drop(key_command);
            }
        };
    } // namespace extra_plugins
} // namespace mavros

PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::KeyboardCommandPlugin, mavros::plugin::PluginBase)

2.修改下图 mavros_plugins.xml 文件

在下图位置添加：

<class name="keyboard_command" type="mavros::extra_plugins::KeyboardCommandPlugin" base_class_type="mavros::plugin::PluginBase">
     <description>Accepts keyboard command.</description>
  </class>

3.修改下图 CMakeLists.txt 文件

在下图位置添加：

  src/plugins/keyboard_command.cpp

4.在下图位置新建​​​ key_command.xml​​​ 文件

 key_command.xml​​​ 中代码如下：

<?xml version="1.0"?>
<mavlink>
    <include>common.xml</include>
  <version>3</version>
  <dialect>0</dialect>
    <enums>
    </enums>
    <messages>
        <message id="229" name="KEY_COMMAND">
	  <description>Keyboard char command.</description>
	 <field type="uint64_t" name="time_usec" units="us">Timestamp</field>
         <field type="float[3]" name="f_a">float32_array</field> 
        </message>
    </messages>
</mavlink>

5.在下图目录下使用MAVLink的图形用户界面代码生成器mavgenerate.py生成MAVLink库文件：

python3 -m mavgenerate

然后在图形界面中选择

XML：~/mavros_ws/src/mavlink/message_definitions/v1.0/key_command.xml​​​
OUT：~/mavros_ws/src/mavlink/message_definitions/v1.0/key_command
Language：C
Protocol：1.0
Validate：勾选
点击Generate按钮

key_command中文件如下：

6.在mavros工作空间下编译mavros

catkin build

五、测试与实现

1.新建ros空间用来发布话题

  创建功能包

~/ros_ws/src
catkin_create_pkg traj_topic roscpp rospy std_msgs mavros mavros_msgs

 创建完成后traj_topic目录如下图所示

 配置 CMakeLists.txt 文件，在下图位置添加：

add_executable(traj src/traj_publisher.cpp)   
target_link_libraries(traj ${catkin_LIBRARIES})

 src目录下新建 traj_publisher.cpp 文件，读取文件的路径改成自己的，代码如下：

#include <ros/ros.h>
#include <mavros_msgs/keyboard_command.h>
#include <time.h>
#include <fstream>
#include <vector>
#include <sstream>

int main(int argc, char **argv)
{
	// ROS节点初始化
	ros::init(argc, argv, "traj_publisher");

	// 创建节点句柄
	ros::NodeHandle n;

    // 创建一个Publisher，发布名为/mavros/keyboard_command/keyboard_sub的topic，消息类型为mavros_msgs::keyboard_command，队列长度10
    ros::Publisher test = n.advertise<mavros_msgs::keyboard_command>("/mavros/keyboard_command/keyboard_sub", 10);

    // 设置循环的频率
	ros::Rate loop_rate(10);
    std::ifstream file("/home/ros/cheng/traj/12/vio_global.csv");
    std::string line;
    std::vector<std::vector<float>> data_rows;
    while (std::getline(file, line))
    {
        std::istringstream iss(line);
        std::vector<float> data_row;
        float file_timestamp, x, y, z;

        // Read timestamp
        iss >> file_timestamp;

        // Read position data (x, y, z)
        iss >> x >> y >> z;

        data_row.push_back(x);
        data_row.push_back(y);
        data_row.push_back(z);

        data_rows.push_back(data_row);

    }
    file.close();
    int count = 0;

    while (ros::ok() && count < data_rows.size())
    {
        ros::spinOnce();
        // 初始化消息
        mavros_msgs::keyboard_command data;

        data.f_a[0] = data_rows[count][0]; 
        data.f_a[1] = data_rows[count][1]; 
        data.f_a[2] = data_rows[count][2]; 

        // 发布消息
        test.publish(data);
        ROS_INFO("Publsh location command[%0.5f, %0.5f, %0.5f]",
                 data.f_a[0], data.f_a[1], data.f_a[2]);

        ++count;
        // 按照循环频率延时
	    loop_rate.sleep();
	}

	return 0;
}

2.编译工作空间

cd ~/ros_ws
catkin_make
source devel/setup.bash

3.飞控连接usb，然后运行：

roslaunch mavros px4.launch fcu_url:=/dev/ttyACM0:921600 gcs_url:=udp-b://@

打开地面站自动连接上，在Mavlink控制台输入receive_data，结果显示如下：

 发布话题：

rosrun traj_topic traj

可查看发布的话题数据：

rostopic echo /mavros/keyboard_command/keyboard_sub

 当然最重要是查看地面站中是否接收到发布的数据，结果如下： 

 至此，PX4飞控接收机载电脑的自定义Mavlink消息成功实现：