一、实现收
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, ¶m);
param.sched_priority = SCHED_PRIORITY_MAX - 80;
(void)pthread_attr_setschedparam(&receiveloop_attr, ¶m);
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。
自定义一个进程用于测试。测试程序就是订阅位置信息并打印。如下: