程序博客网 > 淘宝反作弊部门情况

Mavlink 协议硬解析主要代码

来源：互联网 发布：淘宝反作弊部门情况 编辑：程序博客网 时间：2024/06/05 15:32

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.   
  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 plaincopy
  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 plaincopy
  1. static mavlink_message_info_t g_arMsgInfo[256] = MAVLINK_MESSAGE_INFO  


0 0
淘宝反作弊部门情况 淘宝反作弊部门情况
原创粉丝点击
热门IT博客
玄学通软件下载玄学通软件下载
unity3d中文教程unity3d中文教程
设计电路图软件
电脑连接网络错误651
java处理图片base64
合肥高铁枢纽 知乎
sql语句销量榜
js获取http状态码
vnpy 知乎
mac怎么接有线打印机
手机拍照翻译软件
淘宝网店代运营
dna生物信息数据库
淘宝麻辣烫底料
财经新闻数据
哈登场均数据
生意参谋免费版 知乎
c#如何防止sql注入
那个英语软件好
linux mint vim lua
热门问题 老师的惩罚 人脸识别 我在镇武司摸鱼那些年 重生之率土为王 我在大康的咸鱼生活 盘龙之生命进化 天生仙种 凡人之先天五行 春回大明朝 姑娘不必设防，我是瞎子 背神经离断 背断神经阻断术多少钱 脚断了要多久才会恢复 在冰箱里的女人断脚 断腿 断腿吧 腿断 梦见腿断了 小美人他苏断腿快穿 西塞郑智断腿gif 腿韧带断了能恢复正常 眼镜腿断了502图解 青雉断腿 小狗腿断了自己能好吗 梦见狗腿断了 狗狗腿断了可以自愈吗 塑料眼镜腿断了怎么修 腿断了属于几级伤残 眼镜腿断了眼镜店可以修吗 梦见眼镜腿断了 仓鼠腿断了怎么办 腿断了打钢板多久能好 梦见腿断了是什么预兆 利文斯顿断腿分析图 梦见自己腿断了 我的小断腿 简昂 腿烂 铁链囚禁逃跑断腿 断臂维纳斯 断臂燃身 断臂山 断臂 壮士断臂 杨过断臂 断臂求生 断臂女孩 断臂山结局 断臂维纳斯为什么断臂 断臂刀 断臂山 主角 断臂求法

程序博客网，程序员的互联网技术博客家园。csdn论坛精品 msdn技术资料都在这里