转载:https://mp.weixin.qq.com/s/iGURlSS7V-5iBCEtgpzT7w
一文搞定MAVLINK软件协议
原创 L君 TBUS社区 2019-11-06
本文纯属资深程序猿个人观点,旨在让大家从不同的角度理解MAVLINK协议,文中包含的一些玩笑话,大家不要当真,看着玩玩儿呗~
搞开源无人机的朋友最耳熟能详的莫过于它的通讯协议MAVLINK了。
Mavlink----一个又好气又好笑的名词,仿佛自带光环,它一出场,就会附带两个小弟:ros、mavros。
网络上mavros免费的课程一大堆,mavlink的倒是少之又少,犹如,未过门的媳妇,见不得人啊。
很多人都会形成一种观念,难道是因为mavros要简单些?今天L君要告诉你们,其实直接使用mavlink比使用mavros简单的多啦。
对于一个老程序员来说,理解数据结构犹如吃饭,但是理解框架犹如登天!
先来好好说说这个大哥和两小弟的关系。
ROS
robot operation system,机器人操作系统。说是操作系统,其实跟硬件毫无关系,完全是用一大堆软件堆砌起来的彻头彻尾的----“骗局”。没有mmu、smp、cache,它唯一的功能就是----通讯。把所有的逻辑性代码转换为通讯数据,比如一个函数调用完成的事情,它会拆分成----publish数据、对端接受数据、执行代码。这里看起来,ros跟mavlink似乎毫无关系,直到说到mavros。
MAVROS
一个在ros下的mavlink软件适配包,它的作用其实就是将mavlink协议放到ros框架里去解析,统一使用ros框架,publish、subscribe进行操作。
MAVLINK
一种软件协议,说白了,就是一大堆的struct,做过服务器的朋友应该会搞懂,其实就是封包格式啦,只是mavlink由于介质不唯一,所以加了CRC效验而已。
那么从这里我们已经看出了端倪:学mavros其实是无端的增加了学习成本呀,本来我收发收发数据就完事儿了的东西,我为啥一定要去学一套ros呢?
而且就程序员的原则而言,我们学习时,一定要看到本质,用mavros的时候,中间经过了一个黑盒,里面到底干了什么,我们根本不知道呀。
L君以前也吃过很多亏,用mavros反跟ardupilot代码,发现,我publish出去的东西,人家ardupilot收到的压根儿就不一样!坐标系从NED变成了ENU,然而heading的坐标系还是NED。有些结构体少了几个参数,有的甚至比mavlink原来的还多了几个。
不仅mavros在给你制造着麻烦,ros也时不时给你制造麻烦:
CMakeList怎么改才能用PCL库?
TMD怎么有的包必须用catkin_make_isolated才能编译?
不是说好的catkin_make吗?
为什么我publish出去没效果啊?
catkin_make一分钟,看一次效果3秒。MD怎么用gdb啊?
我好想单步走啊,printf调试起来内心在滴血- -。。。
不!!!这简直就是地狱!我要砍掉它,这不是人干的事儿。我忍受着在linux下用记事本写代码的痛苦,你还给我搞这些幺蛾子?
算了算了,框架这玩意儿不是我等卑微程序员能玩儿的东西,我们还是不怕苦不怕累的啃啃数据流吧。
当我正打算回到小黑屋啃mavlink数据协议的时候,我发现了一个神奇的事情:
一个老程序员的直觉,既然mavlink是以modules存在的,那么一定在那神奇的网站上。。。:
好吧好吧,其实我早就知道了,人家mavlink为了用户方便,sdk早就给我们准备好啦,根本不需要我们自己去做解析,我们快速的git clone下来:
然后我们执行mavgenerate.py脚本:
这里我们XML选择:message_definitions/v1.0/common.xml(其中还有ardupilotmega.xml表示支持APM固件扩展协议,以及其他无人机固件的协议,common表示通用协议,所有固件都支持。)
Out随意选择一个要生成代码的地方
语言选择c语言
协议簇2.0
点击生成:
一个通用的mavlink库就生成好啦
我们现在只需要在编程的时候包含这些头文件就可以实现我们的mavlink通讯了。
现在赶快连上ALICE飞控,打开visual studio:
将我们刚才生成的mav_inc文件夹整个丢进新创建的项目:
并在解决方案资源管理器中添加mavlink的头文件和一个main.cpp(注意文件夹需要自己添加哦):
写入如下代码:
#include <math.h>
#include <thread>
#include <mutex>
#include "mav_inc/mavlink_types.h"
#define MAVLINK_USE_CONVENIENCE_FUNCTIONS
#define MAVLINK_SEND_UART_BYTES(a, b, c) send(a, b, c, 0)
mavlink_system_t mavlink_system = { 1,1 };
#include "mav_inc/common/mavlink.h"
int main(void)
{
Socket sock;
sock.Create();
while (sock.Connect((char*)"127.0.0.1", 1244) != 0)
{
printf("connect error.waiting for 1 second...\n");
usleep(1000000);
}
while (1)
{
mavlink_message_t msg;
mavlink_status_t status;
uint8_t buf[1024];
int recv_len = sock.Receive((char*)buf, 1024);
for (int i = 0; i < recv_len; i++)
{
if (mavlink_parse_char(0, buf[i], &msg, &status))
{
switch (msg.msgid)
{
case MAVLINK_MSG_ID_ATTITUDE:
mavlink_attitude_t attitude;
mavlink_msg_attitude_decode(&msg, &attitude);
printf("rpy=%f,%f,%f\n", attitude.roll, attitude.pitch, attitude.yaw);
break;
case MAVLINK_MSG_ID_HEARTBEAT:
mavlink_heartbeat_t heart;
mavlink_msg_heartbeat_decode(&msg, &heart);
if (heart.base_mode)
{
bool armed = (heart.base_mode & MAV_MODE_FLAG_SAFETY_ARMED);
bool guided = (heart.base_mode & MAV_MODE_FLAG_GUIDED_ENABLED);
printf("armed=%d,guided=%d\n", armed, guided);
}
break;
default:
break;
}
}
}
}
return 0;
}
编译并执行:
可以看到我们已经从mavlink取到当前飞机的姿态信息啦!有着完美的代码变色、自动补全、智能分析排错的visual studio是不是已经让你享受起开发的乐趣了呢?再配合上Alice板卡的wifi网卡功能,你甚至都可以不需要任何辅助,直接实地调试你的程序哟(被逼着插进来的硬核广告)。
现在我们再回过头来开看看代码。
class Socket这个类就不必说了,他是一个tcp协议的公用库类,实现网络通讯功能,不懂的自己百度一下。Alice的ardupilot固件默认使用2号telem作为本地tcp通讯端口,方便开发mavlink外机控制程序。 现在我们先来看main函数里面做了什么,寥寥几行,基本就实现了通讯了:
//实例化网络对象
Socket sock;
//初始化网络对象
sock.Create();
//循环连接本地ip端口1244,alice开机自启动ardupilot使用tcp协议侦听1244号端口发送mavlink
while (sock.Connect((char*)"127.0.0.1", 1244) != 0)
{
//如果连接失败,打印消息,等待1s重试。
printf("connect error.waiting for 1 second...\n");
usleep(1000000);
}
while (1)
{
mavlink_message_t msg;
mavlink_status_t status;
uint8_t buf[1024];//从连接上的tcpsocket收包存到buf中
int recv_len = sock.Receive((char*)buf, 1024);
for (int i = 0; i < recv_len; i++)
{
//这里其实就是最重要最重要的地方了,mavlink_parse_char函数
//第一个参数默认0就行
//第二个参数是一个字节,也就是我们收一次包,比如有20个自己,就循环
//调用20次mavlink_parse_char,把每个字节依次放入
//第三个参数,返回的真实mavlink_raw包
//第四个参数,返回一些正在解析的状态
//如果mavlink_parse_char返回true,表示已经真正收到了一个包,并已
//填充到了msg中。但是我们还不能直接使用这个包,应为现在还是raw形式
//下面还会将它转换成结构体形式!
if (mavlink_parse_char(0, buf[i], &msg, &status))
{
//这个开关语句就是判断收到包的ID号到底是多少啦
//raw形式的msg只能取到ID,无法取到内容
switch (msg.msgid)
{
case MAVLINK_MSG_ID_ATTITUDE:
//判断完ID以后我们就可以直接开始调用函数解析啦
//这里解析的是attitude那么就调用mavlink_msg_attitude_decode
//如果是local_position就是mavlink_msg_local_position_decode
//具体用什么decode你们可以直接到头文件中去查
//mavlink头文件名几乎都是自注释的
mavlink_attitude_t attitude;
mavlink_msg_attitude_decode(&msg, &attitude);
printf("rpy=%f,%f,%f\n", attitude.roll, attitude.pitch, attitude.yaw);
break;
case MAVLINK_MSG_ID_HEARTBEAT:
mavlink_heartbeat_t heart;
//那么这里id是心跳,那么解析函数就是mavlink_msg_heartbeat_decode啦
mavlink_msg_heartbeat_decode(&msg, &heart);
if (heart.base_mode)
{
bool armed = (heart.base_mode & MAV_MODE_FLAG_SAFETY_ARMED);
bool guided = (heart.base_mode & MAV_MODE_FLAG_GUIDED_ENABLED);
printf("armed=%d,guided=%d\n", armed, guided);
}
break;
default:
break;
}
}
}
}
那么,这篇就讲到这里啦,看完全篇,是不是觉得mavlink开发其实是一件非常惬意的事情呢,用对了工具,用对了硬件,才能真正方便我们快速解决问题。
本文选自TBUS论坛_L君原创文章