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

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

mavros功能包能够实现px4飞控和机载电脑之间的实时通信,而对于大部分的消息通信mavros都已经有相应接口可以调用，例如：位置、期望位置、速度、四元素等消息都可以通过C++或者python直接调用。但是,有时候我们想要传输的数据类型在mavros功能包中并没有实现，所以就要求我们需要自定义消息类型来满足数据在飞控和机载电脑之间传输。

首先我们需要明白的是,通信是双向的,即:

                                1:飞控--->机载电脑
                               2: 机载电脑--->飞控

这里我们先实现第一部分:把想要发送的数据类型从px4飞控发送到机载电脑,同时机载电脑需要接收并解析数据.这一部分也包括两个小的部分:飞控发送数据和机载电脑接收数据。

硬件版本:pixhawk4
固件版本:px4 1.11.2
ros版本:1.14.13
mavros版本:1.13.0
机载电脑：TX2 ubuntu18系统

px4飞控发送数据

1.Firmware中自定义消息类型（vehicle_to_tx2.msg）
消息类型的定义需要在固件源码：～/Firmware/msg目录下新建vehicle_to_tx2.msg文件，在其中写上自己需要的消息类型。

#send data from vehicle to tx2.

uint64 timestamp			# time since system start (microseconds)
float32[3] f_u				
float32[3] tau_u			

然后在同一个目录下的Cmakelists.txt文件中添加该msg文件

set(msg_files
			...
			...
			vehicle_to_tx2.msg
)

之后编译的时候会生成一个对应文件名的C++头文件。在使用时需要添加此头文件，为此消息类型赋值，数据就可以在飞控内部传输，这种在飞控内部传输的消息称为uORB消息类型，也可以通过mavlink把该消息传输到机载电脑。

2.Firmware中自定义MAVLink消息
到机载的消息需要通过mavlink发送出去,所以需要在Firmware中自定义一个和前面uORB消息类似的mavlink消息,并使用特定的mavlink生成器生成mavlink库文件,然后添加到发送流中去,Firmware中已经下载了mavlink模块,不需要额外下载.具体步骤如下:
修改~/Firmware/mavlink/include/mavlink/v2.0/message_definitions/common.xml文件，添加自定义消息

<message id="227" name="VEHICLE_TO_TX2">
      <description>send data from veicle to tx2</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_u">float32_array</field>
        <field type="float[3]" name="tau_u">float32_array</field>
    </message>

注意： 这里的id不要和其他id相同,id从227到229可以自己选一个,其他id虽然有可能在common.xml文件中没有使用,但是可能在其他文件中使用了,所以尽量不要使用其他的id,免得和其他的id重复导致编译错误.同时,这里的id是飞控发送数据和机载电脑接收数据之间的桥梁,两个必须匹配才能完成通信。

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

python3 -m mavgenerate
# 也可以使用python3 mavgenerate.py

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

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_vehicle_to_tx2.h

4.添加自定义的mavlink消息到发送流
通过mavlink生成器生成的mavlink消息需要添加到发送流中才能发送出去,相当于我们写了信需要送到邮局去才能给我们寄出去,不然信写得再好别人也收不到.
首先，在~/Firmware/src/modules/mavlink/mavlink_messages.cpp中添加UORB头文件

#include <uORB/topics/vehicle_to_tx2.h>

添加好头文件后,在mavlink_messages.cpp中新建一个消息流的类：

// send data from vehicle to tx2
class MavlinkStreamVehicleToTx2 : public MavlinkStream
{
public:
	const char *get_name() const
	{
		return MavlinkStreamVehicleToTx2::get_name_static();
	}
	static const char *get_name_static()
	{
		return "VEHICLE_TO_TX2";
	}
	static uint16_t get_id_static()
	{
		return MAVLINK_MSG_ID_VEHICLE_TO_TX2;
	}
	uint16_t get_id()
	{
		return get_id_static();
	}
	static MavlinkStream *new_instance(Mavlink *mavlink)
	{
		return new MavlinkStreamVehicleToTx2(mavlink);
	}
	unsigned get_size()
	{
		//return MAVLINK_MSG_ID_VEHICLE_TO_TX2_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
		return data_sub.advertised() ? MAVLINK_MSG_ID_VEHICLE_TO_TX2_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
	}
private:
	uORB::Subscription data_sub{ORB_ID(vehicle_to_tx2)};
	
	// do not allow top copying this class
	MavlinkStreamVehicleToTx2(MavlinkStreamVehicleToTx2 &) = delete;
	MavlinkStreamVehicleToTx2& operator = (const MavlinkStreamVehicleToTx2 &) = delete;
protected:
	explicit MavlinkStreamVehicleToTx2(Mavlink *mavlink) : MavlinkStream(mavlink)
	{}
	bool send(const hrt_abstime t)
	{
		bool data_update = false;
		mavlink_vehicle_to_tx2_t msg{};	     //make sure mavlink_vehicle_to_tx2_t is the definition of your custom mavlink message
		vehicle_to_tx2_s _vehicle_to_tx2;    //make sure vehicle_to_tx2_s is the definition of your uorb topic
		
		if (data_sub.update(&_vehicle_to_tx2)) {
		
			msg.f_u[0] = _vehicle_to_tx2.f_u[0];
			msg.f_u[1] = _vehicle_to_tx2.f_u[1];
			msg.f_u[2] = _vehicle_to_tx2.f_u[2];

			msg.tau_u[0] = _vehicle_to_tx2.tau_u[0];
			msg.tau_u[1] = _vehicle_to_tx2.tau_u[1];
			msg.tau_u[2] = _vehicle_to_tx2.tau_u[2];
			
			data_update = true;
		}
		
		bool data_timeout = (hrt_elapsed_time(&_vehicle_to_tx2.timestamp) > 10_ms);
		if (data_update  && data_timeout) {
			msg.time_usec = hrt_absolute_time();
			mavlink_msg_vehicle_to_tx2_send_struct(_mavlink->get_channel(), &msg);

			return true;
		}
		
		//cout<<"data not update"<<endl;
		return true;
	}
};

在这个类中,会先订阅vehicle_to_tx2这个话题,然后赋值给mavlink_vehicle_to_tx2_t这个类的实例msg,然后再把msg发送出去.然后再把这个定义的MavlinkStreamVehicleToTx2类添加到发送流中去,

create_stream_list_item<MavlinkStreamVehicleToTx2>()

最后在~/Firmware/src/modules/mavlink/mavlink_main.cpp中configure_streams_to_default函数中添加发送频率:

int
Mavlink::configure_streams_to_default(const char *configure_single_stream)
{
switch (_mode) {
case MAVLINK_MODE_NORMAL:
		configure_stream_local("VEHICLE_TO_TX2",20.0f);
case MAVLINK_MODE_ONBOARD:
	configure_stream_local("VEHICLE_TO_TX2",10.0f);
	.
	.
	.
	case MAVLINK_MODE_MINIMAL:
		configure_stream_local("VEHICLE_TO_TX2",10.0f);

若没有没添加发送发送频率,消息也无法从飞控中发送出去.

5.编译
在Firmware目录下执行命令：

make px4_fmu-v5

没有报错说明编译成功.到这里,飞控发送自定义消息这一部分定义完成,下面进行TX2机载电脑接受自定义消息这部分的定义.

机载电脑接收数据

机载通过mavros读取飞控发布的mavlink消息，需要机载上安装ROS(自行安装，需要和ubuntu系统版本一致：ubuntu18对应与ros Melodic版本)，以及mavlink和mavros的源码程序，并且新建与飞控发布的mavlink消息类型相同的mavlink消息类型，通过mavros插件解析获取的mavlink消息，最后还需要新建自定义mavros消息用于接受数据。需要注意的是，这里谈论的mavros与mavlink和前面谈论的Firmware中的mavlink与mavros不完全相同.Firmware中的mavlink与mavros是下载固件时本身就自带的,不需要额下载,同时Firmware中的mavlink是2.0版本,而这里我们谈论的mavlink却是1.0版本的,这是他们之间的一些不同.

1.安装mavlink与mavros
这里我们把本机电脑当成机载电脑,在本机电脑上安装mavros和mavlink,源代码安装:

mkdir -p ~/catkin_ws/src
cd ~/catkin_ws
catkin init
wstool init src
sudo apt-get install python-catkin-tools python-rosinstall-generator -y
wstool init ~/catkin_ws/src
rosinstall_generator --rosdistro kinetic mavlink | tee /tmp/mavros.rosinstall
rosinstall_generator --upstream mavros | tee -a /tmp/mavros.rosinstall
wstool merge -t src /tmp/mavros.rosinstall
 wstool update -t src -j4
 rosdep install --from-paths src --ignore-src -y
./src/mavros/mavros/scripts/install_geographiclib_datasets.sh
catkin build

每个命令的具体含义可以参考:mavros官网安装

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

 <!-- 227  custom message that receive data from vehicle to tx2-->
    <message id="227" name="VEHICLE_TO_TX2">
	    <description>receive data from vehicle to tx2</description>
	    <field type="uint64_t" name="time_usec" units="us">Timestamp</field>
	    <field type="float[3]" name="f_u">float32_array</field>
        <field type="float[3]" name="tau_u">float32_array</field>
    </message>

注意 id一定要一样
在~/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文件夹。

注意： 这里的Protocol选择的是1.0的,而之前选择的是2.0

3.定义mavros消息
在~/catkin_ws/src/mavros/mavros_msgs/msg中自定义msg文件：Vehicle_to_Tx2.msg

# receive data from vehicle to tx2

std_msgs/Header header
float32[3] f_u
float32[3] tau_u

在~/catkin_ws/src/mavros/mavros_msgs/CMakeLists.txt中添加内容：

add_message_files(
  	Vehicle_to_Tx2.msg
)

4.创建MAVROS消息处理插件plugin
机载电脑接收到飞控发送的不同消息类型需要交给不同的插件plugin来处理,具体分给哪个插件就是按前面定义的id来判断的.在~/catkin_ws/src/mavros/mavros_extras/src/plugins创建vehicle_to_tx2.cpp

#include <mavros/mavros_plugin.h>
#include <mavros_msgs/Vehicle_to_Tx2.h>

namespace mavros {
namespace extra_plugins {
class VehicleToTx2Plugin:public plugin::PluginBase {
public:
	VehicleToTx2Plugin():PluginBase(),
		get_data_nh("~PX4_to_TX2")
	{}

	void initialize(UAS &uas_) override {
		PluginBase::initialize(uas_);
		get_data_nh.param<std::string>("frame_id", frame_id, "map");
		
		get_data_pub = get_data_nh.advertise<mavros_msgs::Vehicle_to_Tx2>("receive_data", 10);
	}

	Subscriptions get_subscriptions() override {
		return{
			make_handler(&VehicleToTx2Plugin::handle_get_data)
		};
	}

private:
	ros::NodeHandle get_data_nh;
	ros::Publisher get_data_pub;
	
	std::string frame_id;
	// mavlink::common::msg::VEHICLE_TO_TX2为自动生成的消息头文件中所定义的，也是依据此来解析收到的mavlink消息。
	void handle_get_data(const mavlink::mavlink_message_t *msg, mavlink::common::msg::VEHICLE_TO_TX2 &get_data) {
		auto tmp = boost::make_shared<mavros_msgs::Vehicle_to_Tx2>();
		tmp->header = m_uas->synchronized_header(frame_id, get_data.time_usec);
		tmp->f_u[0] = get_data.f_u[0];
		tmp->f_u[1] = get_data.f_u[1];
		tmp->f_u[2] = get_data.f_u[2];

		tmp->tau_u[0] = get_data.tau_u[0];
		tmp->tau_u[1] = get_data.tau_u[1];
		tmp->tau_u[2] = get_data.tau_u[2];
		
		get_data_pub.publish(tmp);//将解析到的消息发布至topic
	}
};
}
}

#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::VehicleToTx2Plugin, mavros::plugin::PluginBase)

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

	<class name="vehicle_to_tx2" type="mavros::extra_plugins::VehicleToTx2Plugin" base_class_type="mavros::plugin::PluginBase">
		<description>receive data from vehicle to tx2</description>
	</class>

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

add_library(mavros_extras
  src/plugins/vehicle_to_tx2.cpp
）

5.编译
在工作空间catkin_ws下执行命令：

catkin build

测试

在Firmware/src/examples/examples目录中新建2个文件：examples.c和CmakeLists.txt。
在examples.c中分布我们自定义的消息：

#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/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_to_tx2.h>

__EXPORT int examples_main(int argc, char *argv[]);

int examples_main(int argc, char *argv[])
{
	PX4_INFO("test demo");
 
	/* 订阅 sensor_combined主题*/
	int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
	orb_set_interval(sensor_sub_fd, 50);//限制更新频率为20Hz
 
	/* 1.公告attitude主题 */
	struct vehicle_to_tx2_s test;
	memset(&test, 0, sizeof(test));
	orb_advert_t test_pub = orb_advertise(ORB_ID(vehicle_to_tx2), &test);
 
	/* 可以在此等待多个主题 */
	px4_pollfd_struct_t fds[] =
	{
		{ .fd = sensor_sub_fd,   .events = POLLIN },
	};
 
	int error_counter = 0;
 
	while(1)
	{
		/* 等待1000ms获取数据 */
		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)
			{
				/* use a counter to prevent flooding (and slowing us down) */
				PX4_ERR("ERROR return value from poll(): %d", poll_ret);
			}
			error_counter++;
		}
		else    //抓到数据
		{
			if (fds[0].revents & POLLIN)
			{
                /* 复制sensor_combined公告主题 */
				struct sensor_combined_s raw;
				orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw);
 
				PX4_INFO("Accelerometer:\t%8.4f\t%8.4f\t%8.4f",
					 (double)raw.accelerometer_m_s2[0],
					 (double)raw.accelerometer_m_s2[1],
					 (double)raw.accelerometer_m_s2[2]);
 
				/* q在.msg中定义*/
				test.f_u[0] = raw.accelerometer_m_s2[0];
				test.f_u[1] = raw.accelerometer_m_s2[1];
				test.f_u[2] = raw.accelerometer_m_s2[2];

				test.tau_u[0] = raw.gyro_rad[0];
				test.tau_u[1] = raw.gyro_rad[1];
				test.tau_u[2] = raw.gyro_rad[2];
				/*2. 发布vehicle_to_tx2主题*/
				orb_publish(ORB_ID(vehicle_to_tx2), test_pub, &test);
			}
			/* if (fds[1..n].revents & POLLIN) {} */
		}
	}
 
	PX4_INFO("exiting");
	return 0;
}

examples.c文件中为自定义的消息赋值了加速度的值，当然，这个值可以是随便指定的。
在CmakeLists.txt中:

px4_add_module(
	MODULE examples__examples
	MAIN examples
	SRCS
		examples.c 
	DEPENDS
	)

把这个例子加入到飞控中：在/home/zt2s/Firmware/boards/px4/fmu-v5目录下的default.camke文件中添加：

EXAMPLES
			examples

然后编译并刷机：make px4_fmu-v5 upload

飞控上电,用USB连接pixhawk4,打开QGC地面站，新建一个终端，执行下面命令来启动mavros.

roslaunch mavros px4.launch fcu_url:="/dev/ttyTHS2:921600"

当出现下面的情况时,说明插件加载成功.

vehicle_to_tx2 loaded
vehicle_to_tx2 initialized

在工作空间catkin_ws下另开一个终端运行：

source devel/setup.bash
rostopic list

查看有哪些话题发布

/mavros/PX4_to_TX2/receive_data

查看我们定义的话题是否接受到了消息,执行命令:

rostopic echo /mavros/PX4_to_TX2/receive_data

这时还得不到数据，因为飞控端还没有发布数据，这时可以在QGC端的MAVLink Console中输入examples，这样飞控端就能发表数据，而第二个终端中也可以看到数据了。

header: 
	seq: 12
	stamp:
	f_u:
	tau_u:

参考博客