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

相关结构的定义：

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

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

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

版权声明：本文为博主原创文章，未经博主允许不得转载。

Mavlink 协议硬解析主要代码相关推荐

  1. mavlink协议_MAVLink学习之路03_XML中定义MSG并生成C代码

    说明: 本文原创作者『strongerHuang』 首发于微信公众号『嵌入式专栏』,同时也更新在我的个人网站:EmbeddedDevelop 一.写在前面 如果你前面两篇文章掌握了,说明你已经入门了. ...

  2. Mavlink协议概要

    转:http://blog.csdn.net/luckpl/article/details/52608868#mavlink协议简介 Mavlink协议简介 Mavlink协议简介 一Mavlink协 ...

  3. Mavlink协议(第二版)

    文章目录 协议简介 一.Mavlink协议 主要特点 二.数据结构 不兼容标志 (MAVLink 2) 兼容性标志 (MAVLink 2) 有效载荷格式 MAVLink 2 的数据包格式 三.航点协议 ...

  4. 一、Mavlink协议

    一.Mavlink协议 MAVLink通讯协议是一个为微型飞行器设计的非常轻巧的.只由头文件构成的信息编组库.它可以通过串口非常高效地封装C结构数据,并将这些数据包发送至地面控制站.该协议被PX4, ...

  5. mavlink协议详解_MAVLink通讯协议在STM32上移植，并自定义协议

    mavlink全称是(Micro Air Vehicle Message Marshalling Library),从名字可以看出,mavlink是主要面向飞控的一种开源通信协议.因此它默认定义了很多 ...

  6. MAVLink协议详解

    MAVLink是为微型飞行器MAV(Micro Air Vehicle)设计的(LGPL)开源的通讯协议.是无人飞行器和地面站(Ground Control Station ,GCS)之间,以及无人飞 ...

  7. Mavlink协议分析

    阿木社区:转载注意出处 http://www.amovauto.com Pixhawk/APM都是采用MAVLINK协议实现的飞控的数据链路传输.先简单介绍下mavlink协议.Mavlink协议最早 ...

  8. mavlink协议详解_MAVLink学习之路05_ MAVLink应用编程接口分析

    描述 MAVLink应用编程主要内容 2.1 发送和接收说明 利用MAVLink通信协议进行编程,主要实现的功能就是: 1.发送端 将需要发送的数据(如:SysState, BatVol),添加MAV ...

  9. mavlink协议_MAVLink学习之路05_MAVLink应用编程接口分析

    说明: 本文原创作者『strongerHuang』 首发于微信公众号『嵌入式专栏』,同时也更新在我的个人网站:EmbeddedDevelop 一.写在前面 前面写的几篇文章你掌握了,说明你离成功很近了 ...

最新文章

  1. python运行完不能显示图_Python Pygame无法正确显示图像
  2. 使用Docker搭建WordPress博客（三）nginx镜像制作
  3. python如何进行大到小排序_Python3基础 list sort 从小(大)到大(小)排序
  4. shardingsphere启动的时候报错Property ‘sqlSessionFactory‘ or ‘sqlSessionTemplate‘ are required
  5. 自然语言处理中的语言模型预训练方法
  6. boost::mpi模块sendrecv() 操作的测试
  7. 比特币交易手续费_投资人跑步进军比特币，OKEx交易手续费最高打6折
  8. Javascript 检测 页面是否在iframe中
  9. 2017-2018-1 20155227 《信息安全系统设计基础》第四周学习总结
  10. 你需要简单了解JVM中的内存长什么样子
  11. 抢红包案例分析以及代码实现（一） 侵立删
  12. 解决实现虚拟机win10与主机文件的共享问题
  13. php 横屏和竖屏,面试问题，视频横屏与竖屏的设计差异？我是这样回答的
  14. 猛文：关于中国歼20气动性能…
  15. 流媒体协议RTP、RTCP、H264详解
  16. 短视频sdk：选择一个靠谱的短视频SDK 你需要了解这些
  17. FFmpeg获取视频的旋转角度
  18. 实操：基于LNMP搭建zabbix监控
  19. 我的博客园博客开通咯（qyl）
  20. 【比赛回顾】广工2020程序设计初赛C-秋夜hard

热门文章

  1. django解决借用Model使用数据库时报错 AttributeError: Manager isn‘t accessible via xxxxxModel instances
  2. vue 文件转换二进制_Vue利用Blob下载原生二进制数组文件
  3. android 相机应用程序,2020年最佳Android相机的应用程序
  4. arcgis server mysql_ArcGIS Server 10.1发布要素服务时遇到的数据库注册问题总结 (二)...
  5. 未能加载文件或程序集“Newtonsoft.Json, Version=4.5.0.0, Culture=neutral,解决
  6. 省选专练【POI2015】Podzial naszyjnika
  7. 微信小程序开发-微信登陆流程
  8. BZOJ 2763: [JLOI2011]飞行路线 【SPFA】
  9. 响应在此上下文中不可用
  10. Linux内存之Cache