自定义mavlink及收发测试（px4固件V1.11.0，QGC源码4.0）

一、实现收

1.先定义uORB消息  a01_GPS.msg

uint64 timestamp    # time since system start (microseconds)

uint64 lat            # Latitude
uint64 lon            # Longitude

2.在CMakeLists.txt中添加a01_GPS.msg

3.在mavlink_recever.h里加入

#include <uORB/topics/a01_GPS.h>

void handle_message_receive_position_int(mavlink_message_t *msg);

uORB::Publication <a01_GPS_s>                       _a01_GPS_pub{ORB_ID(a01_GPS)};

   第三个代码放在// ORB publications (queue length > 1)  注释后面

 4.

 在mavlink_receiver.cpp文件里添加以下自定义内容。

void
MavlinkReceiver::handle_message(mavlink_message_t *msg)
{
	switch (msg->msgid) {
	case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
		handle_message_receive_position_int(msg);
		break;//自定义
	case MAVLINK_MSG_ID_COMMAND_LONG:
		handle_message_command_long(msg);
		break;

 

void
MavlinkReceiver::receive_start(pthread_t *thread, Mavlink *parent)
{
	pthread_attr_t receiveloop_attr;
	pthread_attr_init(&receiveloop_attr);

	struct sched_param param;
	(void)pthread_attr_getschedparam(&receiveloop_attr, &param);
	param.sched_priority = SCHED_PRIORITY_MAX - 80;
	(void)pthread_attr_setschedparam(&receiveloop_attr, &param);

	pthread_attr_setstacksize(&receiveloop_attr,
				  PX4_STACK_ADJUSTED(sizeof(MavlinkReceiver) + 2840 + MAVLINK_RECEIVER_NET_ADDED_STACK));

	pthread_create(thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent);

	pthread_attr_destroy(&receiveloop_attr);
}
//下面为自定义
void MavlinkReceiver::handle_message_receive_position_int(mavlink_message_t *msg)
{
	a01_GPS_s pos;
	
	mavlink_global_position_int_t msg1 = {};
	mavlink_msg_global_position_int_decode(msg, &msg1);

	if (msg->sysid == 1)
	{
		pos.lat = msg1.lat;
		pos.lon = msg1.lon;
		_a01_GPS_pub.publish(pos);
	}
}

该函数的逻辑很简单，就是接收到位置消息后，调用MAVLINK库中的解包函数进行解析，将解析后的经纬度信息发布出去。然后在别的线程里订阅这个位置消息就行了

5.编译

make px4_fmu-v5_default 

6.测试

src/modules下可自建一个test文件夹，文件夹下包含自己写的.cpp文件与CMakeLists.txt文件。

void main_task::run()
{
    while (1)
    {
       _a01_GPS_sub.copy(&_a01_GPS);

      PX4_INFO("_a01_GPS:%f\r\n",(double)_a01_GPS.lat);
       
      
    }
    
}

px4_add_module(
	#下面添加文件夹名字
	MODULE modules__biandui_position
	#下面添加线程名字
	MAIN a
	STACK_MAIN 10000
	SRCS
	#添加文件夹里面.cpp文件


		
		002mavlink.cpp			#测试mavlink收发      

	DEPENDS

	INCLUDES
		${PX4_SOURCE_DIR}/mavlink/include/mavlink

			COMPILE_FLAGS
		-Wno-cast-align # TODO: fix and enable
		-Wno-address-of-packed-member # TODO: fix in c_library_v2
		#-DDEBUG_BUILD


	)

 在以下目录下添加test,同目录下的debug.cmake文件下也添加test。

 

自定义一个进程用于测试。测试程序就是订阅位置信息并打印。如下：