Mavlink 协议硬解析主要代码

最新推荐文章于 2024-10-24 22:06:40 发布
最新推荐文章于 2024-10-24 22:06:40 发布
阅读量1.1k 收藏 1
点赞数
分类专栏： Drone

2013-08-16 11:26  1744人阅读  评论(1)  收藏  举报
[cpp]  view plain copy
  1. int MAVLinkProtocol::ParseMsg(BYTE arMsgBuf[], MSGVALUE *pMavMsg, CString &strMsgText)  
  2. {  
  3.     // Function    :   
  4.     // Parameters  : arMsgBuf - 为完整的 mavlink msg 缓冲区, 从 0xFE 开始到 最后一个校验字节(CKB)  
  5.     // Return value:   
  6.     // Remark      :   
  7.   
  8.     int  i, nOfst, nLenMaxPayload;  
  9.     BYTE nMsgID;  
  10.     char szTmpName[LEN_MSGNM];  
  11.   
  12.     char szStatusTxt[MAVLINK_MSG_ID_STATUSTEXT_LEN];  
  13.     char szValueID[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN];  
  14.     CString strTmp, strUnicodeTmp;  
  15.   
  16.   
  17.     i = 0;  
  18.     nOfst = 0;  
  19.   
  20.     if(arMsgBuf[0] != 0xfe)  
  21.         return -1;  
  22.   
  23.     nMsgID = arMsgBuf[5];  
  24.     ASSERT(nMsgID < 256);  
  25.   
  26.     nLenMaxPayload = arMsgBuf[1];  
  27.     ASSERT(nLenMaxPayload > 0 && nLenMaxPayload < MAVLINK_MAX_PAYLOAD_LEN);  
  28.   
  29.   
  30.     // 获取 message 字段个数  
  31.     pMavMsg ->nFieldCnts = g_arMsgInfo[nMsgID].num_fields;  
  32.   
  33.     // 获取 message 名称  
  34.     memset(szTmpName, 0x00, sizeof(szTmpName));  
  35.     if(strlen(g_arMsgInfo[nMsgID].name) < LEN_MSGNM)  
  36.         strcpy(szTmpName,  g_arMsgInfo[nMsgID].name);  
  37.     else  
  38.         strncpy(szTmpName, g_arMsgInfo[nMsgID].name, LEN_MSGNM - 1);  
  39.       
  40.     strUnicodeTmp = AnsiStr2Unicode(szTmpName);  
  41.     if(strUnicodeTmp.GetLength() > LEN_MSGNM - 1)  
  42.         strUnicodeTmp = strUnicodeTmp.Left(LEN_MSGNM - 1);  
  43.     _tcscpy(pMavMsg ->szMsgName, strUnicodeTmp);  
  44.   
  45.       
  46.   
  47.     // 根据 g_arMsgInfo[] 获取并处理各字段信息  
  48.     for(i = 0; i < pMavMsg ->nFieldCnts; i++)  
  49.     {  
  50.         strUnicodeTmp = AnsiStr2Unicode((char *) g_arMsgInfo[nMsgID].fields[i].name);  
  51.           
  52.         if(strUnicodeTmp.GetLength() > LEN_FIELDNM - 1)  
  53.             strUnicodeTmp = strUnicodeTmp.Left(LEN_FIELDNM - 1);  
  54.           
  55.         _tcscpy(pMavMsg ->arField[i].szFieldNm, strUnicodeTmp);                              // Field Name  
  56.         pMavMsg ->arField[i].nFieldTyp = g_arMsgInfo[nMsgID].fields[i].type;             // Field Type  
  57.           
  58.         nOfst = g_arMsgInfo[nMsgID].fields[i].wire_offset;  
  59.   
  60.   
  61.         // 特殊消息的处理  
  62.         if(nMsgID == MAVLINK_MSG_ID_STATUSTEXT && i == 1)             
  63.         {  
  64.             memset(szStatusTxt, 0x00, sizeof(szStatusTxt));  
  65.             memcpy(szStatusTxt, (arMsgBuf + MAVLINK_NUM_HEADER_BYTES + 1), MAVLINK_MSG_ID_STATUSTEXT_LEN);      // 1 为 字符串位置相对载荷开始位置的偏移  
  66.             strMsgText = AnsiStr2Unicode(szStatusTxt);  
  67.         }  
  68.         else if(nMsgID == MAVLINK_MSG_ID_PARAM_VALUE && i == 3)  
  69.         {  
  70.             memset(szValueID, 0x00, sizeof(szValueID));  
  71.             memcpy(szValueID, (arMsgBuf + MAVLINK_NUM_HEADER_BYTES + 8), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);  
  72.             strMsgText = AnsiStr2Unicode(szValueID);  
  73.         }  
  74.         else if(nMsgID == MAVLINK_MSG_ID_PARAM_SET && i == 3)  
  75.         {  
  76.             memcpy(szValueID, (arMsgBuf + MAVLINK_NUM_HEADER_BYTES + 6), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);  
  77.             strMsgText = AnsiStr2Unicode(szValueID);  
  78.         }  
  79.         else  
  80.         {  
  81.             // 一般 mavlink msg 的处理  
  82.   
  83.             // 获取每个数值所存储的缓冲区  
  84.             if(g_arMsgInfo[nMsgID].fields[i].type == MAVLINK_TYPE_CHAR ||   
  85.                g_arMsgInfo[nMsgID].fields[i].type == MAVLINK_TYPE_UINT8_T ||   
  86.                g_arMsgInfo[nMsgID].fields[i].type == MAVLINK_TYPE_INT8_T)  
  87.             {  
  88.                 pMavMsg ->arField[i].arData[0] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst];  
  89.                 ASSERT(nOfst < nLenMaxPayload);  
  90.             }  
  91.             else if(g_arMsgInfo[nMsgID].fields[i].type == MAVLINK_TYPE_UINT16_T ||   
  92.                 g_arMsgInfo[nMsgID].fields[i].type == MAVLINK_TYPE_INT16_T)  
  93.             {  
  94.                 pMavMsg ->arField[i].arData[0] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst];  
  95.                 pMavMsg ->arField[i].arData[1] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 1];  
  96.                   
  97.                 ASSERT(nOfst + 1 < nLenMaxPayload);  
  98.             }  
  99.             else if(g_arMsgInfo[nMsgID].fields[i].type == MAVLINK_TYPE_UINT32_T ||   
  100.                 g_arMsgInfo[nMsgID].fields[i].type == MAVLINK_TYPE_INT32_T)  
  101.             {  
  102.                 pMavMsg ->arField[i].arData[0] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst];  
  103.                 pMavMsg ->arField[i].arData[1] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 1];  
  104.                 pMavMsg ->arField[i].arData[2] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 2];  
  105.                 pMavMsg ->arField[i].arData[3] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 3];  
  106.                   
  107.                 ASSERT(nOfst + 3 < nLenMaxPayload);  
  108.             }  
  109.             else if(g_arMsgInfo[nMsgID].fields[i].type == MAVLINK_TYPE_UINT64_T ||  
  110.                 g_arMsgInfo[nMsgID].fields[i].type == MAVLINK_TYPE_INT64_T)  
  111.             {  
  112.                 pMavMsg ->arField[i].arData[0] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst];  
  113.                 pMavMsg ->arField[i].arData[1] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 1];  
  114.                 pMavMsg ->arField[i].arData[2] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 2];  
  115.                 pMavMsg ->arField[i].arData[3] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 3];  
  116.                 pMavMsg ->arField[i].arData[4] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 4];  
  117.                 pMavMsg ->arField[i].arData[5] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 5];  
  118.                 pMavMsg ->arField[i].arData[6] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 6];  
  119.                 pMavMsg ->arField[i].arData[7] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 7];  
  120.                   
  121.                 ASSERT(nOfst + 7 < nLenMaxPayload);  
  122.             }  
  123.             else if(g_arMsgInfo[nMsgID].fields[i].type == MAVLINK_TYPE_FLOAT)           // 4 byte  
  124.             {  
  125.                 pMavMsg ->arField[i].arData[0] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst];  
  126.                 pMavMsg ->arField[i].arData[1] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 1];  
  127.                 pMavMsg ->arField[i].arData[2] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 2];  
  128.                 pMavMsg ->arField[i].arData[3] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 3];  
  129.                   
  130.                 ASSERT(nOfst + 3 < nLenMaxPayload);  
  131.             }  
  132.             else if(g_arMsgInfo[nMsgID].fields[i].type == MAVLINK_TYPE_DOUBLE)      // 8 byte  
  133.             {  
  134.                 pMavMsg ->arField[i].arData[0] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst];  
  135.                 pMavMsg ->arField[i].arData[1] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 1];  
  136.                 pMavMsg ->arField[i].arData[2] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 2];  
  137.                 pMavMsg ->arField[i].arData[3] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 3];  
  138.                 pMavMsg ->arField[i].arData[4] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 4];  
  139.                 pMavMsg ->arField[i].arData[5] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 5];  
  140.                 pMavMsg ->arField[i].arData[6] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 6];  
  141.                 pMavMsg ->arField[i].arData[7] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 7];   
  142.                   
  143.                 ASSERT(nOfst + 7 < nLenMaxPayload);  
  144.             }  
  145.             else   
  146.             {  
  147.                 TRACE(_T("\r\n> MAVLinkProtocol.ParseMsg - Unexpect field type: "));  
  148.                 TRACE(_T("\r\n> MAVLinkProtocol.ParseMsg - g_arMsgInfo[nMsgID].fields[i].type = %d"), g_arMsgInfo[nMsgID].fields[i].type);  
  149.             }  
  150.         }  
  151.     }  
  152.   
  153.     return nMsgID;  
  154. }  


相关结构的定义：

[cpp]  view plain copy
  1. struct FIELDSVALUE  
  2. {  
  3.     TCHAR   szFieldNm[LEN_FIELDNM];  
  4.     uint8_t nFieldTyp;  
  5.     BYTE    arData[8];                                      // 字段数值  
  6. };  
  7.   
  8. struct MSGVALUE  
  9. {  
  10.     TCHAR   szMsgName[LEN_MSGNM];  
  11.     uint8_t nFieldCnts;  
  12.       
  13.     FIELDSVALUE arField[MAVLINK_MAX_FIELDS];  
  14. };  


MAVLINK_MESSAGE_INFO 的定义见 mavlink\v1.0\common\common.h

[cpp]  view plain copy
  1. static mavlink_message_info_t g_arMsgInfo[256] = MAVLINK_MESSAGE_INFO  


bbzz2
关注
专栏目录
深入解析MAVLink 1协议在STM32上的应用：从外部命令到自定义数据处理的完整实现指南
qq_38334677的博客
09-03 855
在无人机和机器人系统的开发过程中，通信协议的选择至关重要。MAVLink（Micro Air Vehicle Link）是广泛应用于无人机和机器人系统的轻量级通信协议，特别是在飞行控制系统和地面站之间的数据传输中。它提供了可靠且高效的数据交换机制，用于传输位置、姿态、传感器读数等信息，并允许远程操控设备执行复杂的任务。尽管MAVLink 2已经发布，但MAVLink 1仍然在许多项目中得到广泛支持。
Mavlink协议的C#动态库及源码
01-07
地面站C#源码开发中可直接调用的动态库及源码，解决了mavgenerate生成C#时的bug
Mavlink地面站编写之一--协议分析
AMOVLAB(阿木实验室)
03-28 2万+
Pixhawk/APM都是采用MAVLINK协议实现的飞控的数据链路传输。先简单介绍下mavlink协议。Mavlink协议最早由 苏黎世联邦理工学院 计算机视觉与几何实验组 的 Lorenz Meier于2009年发布，并遵循LGPL开源协议。Mavlink协议是在串口通讯基础上的一种更高层的开源通讯协议，主要应用在微型飞行器（micro aerial vehicle）的通讯上。Mavlink是
【无人机开发】通讯协议MavLink详解
热门推荐
BrilliantEagle的专栏
03-08 2万+
1. MAVLink简介MAVLink（Micro Air Vehicle Link，微型空中飞行器链路通讯协议）是无人飞行器与地面站（Ground Control Station ，GCS）之间通讯，以及无人飞行器之间通讯最常用的协议。它已经在PX4、APM、PIXHAWK和Parrot AR.Drone飞控平台上进行了大量测试。2.发明者Lorenz Meier简介MAVLink的最初开发于20
MAVLink协议通信分析——（一）概述
Fergus
08-28 2531
第一章       概述 MavLink（Micro AirVehicle Link）是一种用于小型无人载具的通信协议，于2009年首次发布。该协议广泛应用于地面站（Ground Control Station，GCS）与无人载具（Unmanned vehicles）之间的通信，同时也应用在载具内部子系统的内部通信中，协议以消息库的形式定义了参数传输的规则。MavLink协议支持无人固定
mavlink协议解析
王尼莫的博客
07-11 763
接下来的两个字节通常用于表示发送消息的系统ID和组件ID。在这个例子中，48可能是系统ID，FF可能是组件ID，或者反之。这个字节表示数据包中有效载荷（不包括起始字节、长度、系统ID、组件ID和校验和）的长度。BE在十六进制中对应的十进制数值是190，根据MAVLink的消息ID定义，这个值对应于特定的消息类型。在这个例子中，序列号为00 00，可能表示这是一个逻辑上的起始消息。在这个例子中，我们有6个字节的数据。这是MAVLink协议的起始标志位，在MAVLink v1中，每个数据包都以0xFE开始。
mavlink协议收发源码
01-07
MAVLink协议是无人机和机器人系统中广泛使用的轻量级通信协议，它允许设备之间高效、可靠地交换数据。在这个“mavlink协议收发源码”中，我们重点关注的是如何在C++环境中利用MAVLink协议进行数据的发送和接收。下面...
mavlink协议详解_MAVLink通讯协议在STM32上移植，并自定义协议
weixin_35470511的博客
02-07 3437
mavlink全称是(Micro Air Vehicle Message Marshalling Library)，从名字可以看出，mavlink是主要面向飞控的一种开源通信协议。因此它默认定义了很多适用于飞控的信息格式，比如heartbeat(心跳信号，每隔一两秒主从通信一次，以验证通信是否正常)。首先要说明的是，mavlink作为一个非常可靠(至少两字节校验)、支持类型丰富(message I...
Mavlink协议生成工具 便携版.7z
04-27
其主要特点是紧凑的数据包格式，允许快速解析和低带宽占用，这对于空中通信至关重要。协议中定义了各种消息类型，包括位置、姿态、传感器数据等，使得不同硬件和软件平台能够无缝交互。 "Mavlink协议生成工具"则是...
Mavlink通信协议Java版本
09-26
**Mavlink通信协议Java版本**是针对无人机爱好者和开发者的一款重要工具，它基于Mavlink协议，并已转化为适用于Java编程环境的实现。Mavlink协议是无人机领域广泛使用的轻量级、高效的数据通信协议，它能确保飞行器...
Mavlink协议
pengl的博客
09-21 2万+
Mavlink协议简介Mavlink协议简介 一Mavlink协议 二数据结构 三协议支持的数据类型 四性能 五航点协议 读取MAV的航点列表 写MAV的航点列表 清除MAV的航点列表 设置当前MAV航点 六参数的读写 读取参数列表 读取单个参数 写参数 七增加新的mavlink消息 XML文件中消息的定义 建立消息定义文件 编绎XML文件 八消息的发送与解析 九通用的Mavlink消息 参考资料
MAVLink分析及封装自定义的类MavLink通信协议
wangyongyao1989的博客
03-29 2198
通过自动生成mavlink的协议代码，我们可以此基础上进行自定义的拓展。形成自己通信协议。··首先我们把协议通信命令区分为发送和接收：AckMsg和CmdMsg两类，分别用于接收和发送。AckSysInfo类会解析出playload的属性值：//解析出payload包含的实体数值@Override'}';CmdSysInfo会包playload的属性值包装成一个MyLinkPacket：@Override。
MAVlink协议 部分通用消息集解析
最新发布
Bit_Dong的博客
10-24 1074
MAVLink是一种非常轻量级的消息传输协议, 用于地面控制终端（地面站）与无人机之间 (以及机载无人机组件之间) 进行通信， 为一种设计用于资源受限系统及带宽受限链路的二进制遥测协议。
Unity3d C#解析Mavlink
zouxin_88的专栏
08-17 967
1.编译Mavlink（MissionPlanner-master\ExtLibs\Mavlink），生成MAVLink.dll 2.编译过程中可能遇到“使用来自文件“mykey.snk”的公钥对输出签名时出错”的错误，在工程属性中不勾选签名。 3.将生成的MAVLink.dll和Newtonsoft.Json.dll复制到Unity3d工程的Assets/Plugins文件下 4.将以下代码挂载到相机 using Mavlink.Common; using Mavlink.Cor..
mavlink解析
shanglizhangrui的专栏
10-21 1万+
之前看了mavlink协议，网上关于mavlink的资料不多。本系列共三篇，这是第一篇。本文大概总结了下对mavlink协议的理解。以下如不说明都是说mavlink v1.0版本。 首先附上mavlink的各个消息的简介https://pixhawk.ethz.ch/mavlink/（这里的内容很多，建议大概了解mavlink后再去浏览），mavlink协议介绍http://qgroun
MAVLink协议解析
06-05
MAVLink协议解析
09 Mavlink协议移植.zip
02-12
本篇文章将深入探讨如何将Mavlink协议移植到STM32微控制器上，并结合实际应用，解析其在单片机开发中的关键点。 首先，理解Mavlink协议的基础是至关重要的。Mavlink是一种基于消息结构的数据传输协议，它的设计目标...
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值