px4使用mavlink和其他无人机通信并实现跟随飞行

超维空间科技

已于 2023-07-02 09:25:12 修改

阅读量1.9k

点赞数 1

分类专栏：调试笔记文章标签： PX4 mavlink

于 2020-07-18 19:41:16 首次发布

本文链接：https://blog.csdn.net/qq_38768959/article/details/107434044

版权

调试笔记专栏收录该内容

39 篇文章

订阅专栏

交流学习加qq:2096723956
PX4固件版本1.11.0
以位置信息为例，本文直接使用MAVLINK库中预定义的位置消息，如果想自定义MAVLINK消息，参考
https://blog.csdn.net/qq_38768959/article/details/108802979?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522163894817116780269899703%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fblog.%2522%257D&request_id=163894817116780269899703&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2_blogfirst_rank_v2~rank_v29-3-108802979.pc_v2_rank_blog_default&utm_term=mavlink&spm=1018.2226.3001.4450
先定义位置UORB消息，记得在cmakelist里也添加一下

在这里插入图片描述

在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)};

其中handle_message_receive_position_int是位置信息的接收处理函数，_a01_GPS_pub用于发布接收到的位置信息。

在mavlink_recever.cpp里的handle_message函数里加入handle_message_receive_position_int处理函数，如下图，在无人机接收到位置信息后进入位置接收处理函数

在这里插入图片描述
然后在mavlink_recever.cpp中定义handle_message_receive_position_int
如下：

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库中的解包函数进行解析，将解析后的经纬度信息发布出去。然后在别的线程里订阅这个位置消息就行了。
这里自定义一个进程用于测试。测试程序就是订阅位置信息并打印。如下：

a01_GPS_s _a01_GPS;
uORB::Subscription _a01_GPS_sub{ORB_ID(a01_GPS)};
_a01_GPS_sub.copy(&_a01_GPS);
 PX4_INFO("_a01_GPS:%f\r\n",(double)_a01_GPS.lat);

测试
用一架ID为1的无人机和一架ID为其他的无人机，用数传建立通信。将ID为其他的无人机用USB连接到地面站。在ID为其他的无人机上运行测试进程。观察地面站终端的打印信息如下：

在这里插入图片描述
可以看到已经成功的接收到ID为1的无人机的纬度。接收到的信息是实际的纬度乘以10的七次方的。

如果想进一步的利用1号无人机的位置实现对一号无人机的跟踪。
可以利用以下程序：

_vehicle_local_position_sub.copy(&_vehicle_local_position);
globallocalconverter_init(_vehicle_local_position.ref_lat,_vehicle_local_position.ref_lon, _vehicle_local_position.ref_alt, hrt_absolute_time());
_a01_GPS_sub.copy(&_a01_GPS);
        _ref_lat=(double)_a01_GPS.lat/10000000;
        _ref_lon=(double)_a01_GPS.lon/10000000;
 globallocalconverter_tolocal(_ref_lat,_ref_lon,30, &x1, &y1, &z1);
             _pos_sp_triplet.current.x =x1+5;
            _pos_sp_triplet.current.y =y1;
            _pos_sp_triplet.current.z =-3.0f;

其中globallocalconverter_init是PX4自带的位置转换初始化函数。初始化完成后利用globallocalconverter_tolocal将1号无人机的经纬度转换为以初始化经纬度为原点的本地坐标。再将本地坐标加上偏移量赋给位置控制三组合去执行，相当于实时的以一号无人机为原点，保持一个相对的偏移量，从而实现对一号无人机的跟踪飞行。