px4飞控和机载电脑通信：飞控接收机载电脑的自定义mavlink消息

前面一篇讲了机载电脑怎么接受飞控的px4消息.这一篇讲解如何飞控怎么接收从机载电脑传过来的消息.分成两部分:机载电脑发送消息,飞控接收消息.

pixhawk版本:pixhawk4
px4版本:1.11.2
ros版本:1.14.10
机载电脑：TX2 ubuntu18系统

mavros发送消息

1.创建msg消息
参考前一篇文章的文件路径,首先需要在机载电脑中的~/catkin_ws/src/mavros/mavros_msgs/msg文件夹下新建Tx2_to_Vehicle.msg文件,该文件就是我们要传输的消息的数据类型,然后在文件中写入:

std_msgs/Header header
float32[3] f_a
float32[3] tau_a

然后在~/catkin_ws/src/mavros/mavros_msgs目录下的CmakeLists.txt文件中添加Tx2_to_Vehicle.msg文件

add_message_files(
....
Tx2_to_Vehicle.msg
)

然后使用:catkin build 命令在~/catkin_ws目录下编译一下,确保在mavros_msgs目录下生成了Tx2_to_Vehicle.h头文件,方便后续文件的引入.

2.添加自定义的mavros消息

修改~/catkin_ws/src/mavlink/message_definitions/v1.0/common.xml文件，添加自定义消息：

<message id="14226" name="TX2_TO_VEHICLE">
	  <description>the message from tx2 to vehicle</description>
	  <field type="uint64_t" name="time_usec" units="us">Timestamp</field>
      <field type="float[3]" name="f_a">float32_array</field>
      <field type="float[3]" name="tau_a">float32_array</field>      
</message>

在~/catkin_ws/src/mavlink/message_definitions目录下,使用MAVLink的图形用户界面代码生成器mavgenerate.py生成MAVLink库文件：

python3 -m mavgenerate

然后在图形界面中选择

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

为了将消息发送出去,我们需要编写一个插件.~/catkin_ws/src/mavros/mavros_extras/src/plugins目录下创建Tx2_to_Vehicle.cpp文件

#include <mavros/mavros_plugin.h>
#include <pluginlib/class_list_macros.h>
#include <iostream>
#include <mavros_msgs/Tx2_to_Vehicle.h>

namespace mavros {
namespace extra_plugins{

class Tx2ToVehiclePlugin : public plugin::PluginBase {
public:
     Tx2ToVehiclePlugin() : PluginBase(),
         nh("~Tx2_to_Px4"){ };

     void initialize(UAS &uas_)
     {
         PluginBase::initialize(uas_);
         mavros2fcu_sub = nh.subscribe("send_data", 10, &Tx2ToVehiclePlugin::Tx2ToVehicle_cb, this);
     };

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

 private:
     ros::NodeHandle nh;
     ros::Subscriber send_sub;

    void Tx2ToVehicle_cb(const mavros_msgs::Tx2_to_Vehicle::ConstPtr &req)
     {
        mavros::UAS *m_uas_ = static_cast<Tx2ToVehiclePlugin *>(this)->m_uas;

		mavlink::common::msg::	TX2_TO_VEHICLE send_data = {}; 

		//官方教程中没有这句,所以会报错,因为send_message_ignore_drop的参数是message消息类型,
		//所以需要先用python mavgenerate.py生成mavlink::common::msg::KEY_COMMANDS
		send_data.f_a[0] = req->f_a[0];
		send_data.f_a[1] = req->f_a[1];
		send_data.f_a[2] = req->f_a[2];
		
		send_data.tau_a[0] = req->tau_a[0];
		send_data.tau_a[1] = req->tau_a[1];
		send_data.tau_a[2] = req->tau_a[2];
        //std::cout << "Got data : " << req->a << req->b << req->c << std::endl;
        UAS_FCU(m_uas)->send_message_ignore_drop(send_data);
     }
};
}   // namespace extra_plugins
}   // namespace mavros

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

将自定义的插件添加到插件列表中，用于MAVROS自启动插件：
~/catkin_ws/src/mavros/mavros_extras/mavros_plugins.xml中添加：

	<class name="mavros2fcu" type="mavros::extra_plugins::Tx2ToVehiclePlugin" base_class_type="mavros::plugin::PluginBase">
		<description>send mavros to fcu</description>
	</class>

在~/catkin_ws/src/mavros/mavros_extras/CMmakeLists.txt添加编译信息

add_library(mavros_extras
  src/plugins/Tx2_to_Vehicle.cpp
）

5.编译

在工作空间catkin_ws下执行命令：

catkin build

6.手动发布数据
6.1创建ros工作空间
插件Tx2_to_Vehicle.cpp中会订阅数据并发布出去,最开始时是订阅不到数据的,因为数据还没有从其他的模块中发布,为了演示程序的正确性,所以我们这里需要手动的发布一些数据,让这个模块订阅然后发送给飞控.步骤如下:

mkdir ros_demo      //创建工作空间
cd ros_demo
mkdir src     //创建子空间
cd src
catkin_create_pkg demo //创建包名,该命令能够在src/demo目录下生成package.xml和CMakeLists.txt

6.2新建发布文件
在demo目录下新建一个pub_data.cpp文件,在该文件中写入:

#include <time.h>
#include <ros/ros.h>
#include <mavros_msgs/Mavros2Fcu.h>

using namespace std;
mavros_msgs::Mavros2Fcu data;
int main(int argc, char **argv){
	ros::init(argc, argv, "ss");
    ros::NodeHandle nh;
	float aa = 0;

	data.f_a[0] = aa+1;
	data.f_a[1] = aa+2;
	data.f_a[2] = aa+3;
	
	data.tau_a[0] = aa+1;
	data.tau_a[1] = aa+2;
	data.tau_a[2] = aa+3;
	
	ros::Publisher test;
	test = nh.advertise<mavros_msgs::Tx2_to_Vehicle>("/mavros/Tx2_to_Px4/send_data", 10);

	ros::Rate rate(20.0);
	while(ros::ok()){
		ros::spinOnce();
		rate.sleep();
		test.publish(data);
		aa = aa + 1;
		//cout<<data.a<<endl;
    }
	return 0;
}

该文件能够发布Tx2_to_Vehicle类型的消息,以便于Tx2_to_Vehicle.cpp这个插件订阅.

6.3配置package.xml和CMakeLists.txt

写好发布文件之后,还需要在package.xml和CMakeLists.txt中配置一下才能使用.
在CmakeLists.txt文件中添加:

find_package(catkin REQUIRED COMPONENTS
  mavros
  roscpp
  std_msgs
  mavros_msgs
)

add_executable(a src/pub_data.cpp)   //a是文件执行名,可以随便取
target_link_libraries(a ${catkin_LIBRARIES})

在package.xml中添加:

  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>mavros</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>mavros_msgs</build_depend>

  <build_export_depend>mavros</build_export_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>

  <exec_depend>mavros</exec_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>mavros_msgs</exec_depend>

最后编译一下:catkin_make

px4接收消息

1.添加uorb消息
在飞控端接收机载电脑传来的数据,需要定义uorb消息来接收.在~/Firmware/msg目录下创建tx2_to_vehicle.msg文件,并写入:

uint64 timestamp			# time since system start (microseconds)

float32[3] f_a				
float32[3] tau_a			

然后在~/Firmware/msg目录下的CMakeLists.txt文件中添加tx2_to_vehicle.msg文件

set(msg_files
...
...
tx2_to_vehicle.msg
)

2.自定义mavlink消息
在~/Firmware/mavlink/include/mavlink/v2.0/message_definitions/common.xml文件中,添加自定义消息

<message id="14226" name="TX2_TO_VEHICLE">
<description>receive data from tx2 to vehicle</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>
<field type="float[3]" name="tau_a">float32_array</field>
</message>

这里的id和mavros端的id一致.

3.生成mavlink库文件

使用MAVLink的图形用户界面代码生成器mavgenerate.py生成MAVLink库文件使用MAVLink的图形用户界面代码生成器mavgenerate.py生成MAVLink库文件,在~/Firmware/mavlink/include/mavlink/v2.0目录下打开终端并执行:

python3 -m mavgenerate

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

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

可以在~/Firmware/mavlink/include/mavlink/v2.0/中生成的common文件夹中能找到生成的.h文件：
mavlink_msg_tx2_to_vehicle.h

4.修改mavlink_receiver.h文件
在~/Firmware/src/modules/mavlink目录下的mavlink_receiver.h中添加下面内容:

...
#include <uORB/topics/tx2_to_vehicle.h>
...
class MavlinkReceiver
{
...
private:
    void handle_message_tx2_to_vehicle(mavlink_message_t *msg);
...
	orb_advert_t _tx2_to_vehicle_pub{nullptr};
	

在~/Firmware/src/modules/mavlink目录下的mavlink_receiver.cpp文件中添加:

...
void MavlinkReceiver::handle_message(mavlink_message_t *msg)
{
...
	case MAVLINK_MSG_ID_TX2_TO_VEHICLE:
		handle_message_tx2_to_vehicle(msg);
		break;
...
}
...
void MavlinkReceiver::handle_message_tx2_to_vehicle(mavlink_message_t *msg){
    mavlink_tx2_to_vehicle_t man;
    mavlink_msg_tx2_to_vehicle_decode(msg, &man);
    struct tx2_to_vehicle_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];

    data.tau_a[0] = man.tau_a[0];
    data.tau_a[1] = man.tau_a[1];
    data.tau_a[2] = man.tau_a[2];

    if (_tx2_to_vehicle_pub == nullptr) {
        _tx2_to_vehicle_pub = orb_advertise(ORB_ID(tx2_to_vehicle), &data);

    } else {
        orb_publish(ORB_ID(tx2_to_vehicle), _tx2_to_vehicle_pub, &data);
    }

}

5.订阅模块
在~/Firmware/src/modules目录下创建一个文件夹receive_data.并在该文件夹下创建CMakeLists.txt文件和receive_data.cpp文件.在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/tx2_to_vehicle.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(tx2_to_vehicle));
   orb_set_interval(data_sub_fd, 200); // limit the update rate to 200ms

   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 tx2_to_vehicle_s input;
		orb_copy(ORB_ID(tx2_to_vehicle), data_sub_fd, &input);
		PX4_INFO("Recieved data a,b,c : %lf %lf %lf", (double)input.f_a[0], (double)input.f_a[1], (double)input.f_a[2]);
	    }
	}
    }
    return 0;
}

在CMakeLists.txt文件中添加:

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

把该模块添加到系统的配置文件中去,在~/Firmware/boards/px4/fmu-v5/default.cmake中添加:

MODULES
     ...
     receive_data
     ...

也可以添加到~/Firmware/boards/px4/sitl/default.cmake中,可以放到模拟仿真中去验证.

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

make px4_fmu-ve uplod

测试

在机载电脑端终端中启动roscore，再另外打开一个终端执行roslaunch mavros px4.launch，另外打开一个终端，执行rostopic echo /mavros/state查看connected是否为true，为true表示连接上了，反之则没有。
连接上之后就可以执行rosrun demo pub_data发送数据。
用USB连接上地面站，在地面站中执行receive_data来收集数据，如下图：

当关闭rosrun demo pub_data后就会出现Got no data within a second的错误，表明数据已经发过来了，机载电脑端停止发送后，飞控就收不到数据。

以上就是px4接收自定义mavros消息的内容.