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文件

 在下图位置添加

     Keyboard char command.
     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.
     float32_array
   

修改下图 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 

 下图位置添加：

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 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

#include 
#include 

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 
#include 
#include 
#include 
#include 

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 文件

在下图位置添加：

     Accepts keyboard command.
  

3.修改下图 CMakeLists.txt 文件

在下图位置添加：

  src/plugins/keyboard_command.cpp

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

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

    common.xml
  3
  0
    
    
    
        
	  Keyboard char command.
	 Timestamp
         float32_array 
        
    

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 
#include 
#include 
#include 
#include 
#include 

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/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> data_rows;
    while (std::getline(file, line))
    {
        std::istringstream iss(line);
        std::vector 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消息成功实现：