Mavlink 协议硬解析主要代码



2013-08-16 11:26  1744人阅读  评论(1)  收藏  举报

 int MAVLinkProtocol::ParseMsg(BYTE arMsgBuf[], MSGVALUE *pMavMsg, CString &strMsgText)
 {
     // Function    :
     // Parameters  : arMsgBuf - 为完整的 mavlink msg 缓冲区, 从 0xFE 开始到 最后一个校验字节(CKB)
     // Return value:
     // Remark      :

     int  i, nOfst, nLenMaxPayload;
     BYTE nMsgID;
     char szTmpName[LEN_MSGNM];

     char szStatusTxt[MAVLINK_MSG_ID_STATUSTEXT_LEN];
     char szValueID[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN];
     CString strTmp, strUnicodeTmp;


     i = 0;
     nOfst = 0;

     if(arMsgBuf[0] != 0xfe)
         return -1;

     nMsgID = arMsgBuf[5];
     ASSERT(nMsgID < 256);

     nLenMaxPayload = arMsgBuf[1];
     ASSERT(nLenMaxPayload > 0 && nLenMaxPayload < MAVLINK_MAX_PAYLOAD_LEN);


     // 获取 message 字段个数
     pMavMsg ->nFieldCnts = g_arMsgInfo[nMsgID].num_fields;

     // 获取 message 名称
     memset(szTmpName, 0x00, sizeof(szTmpName));
     if(strlen(g_arMsgInfo[nMsgID].name) < LEN_MSGNM)
         strcpy(szTmpName,  g_arMsgInfo[nMsgID].name);
     else
         strncpy(szTmpName, g_arMsgInfo[nMsgID].name, LEN_MSGNM - 1);

     strUnicodeTmp = AnsiStr2Unicode(szTmpName);
     if(strUnicodeTmp.GetLength() > LEN_MSGNM - 1)
         strUnicodeTmp = strUnicodeTmp.Left(LEN_MSGNM - 1);
     _tcscpy(pMavMsg ->szMsgName, strUnicodeTmp);



     // 根据 g_arMsgInfo[] 获取并处理各字段信息
     for(i = 0; i < pMavMsg ->nFieldCnts; i++)
     {
         strUnicodeTmp = AnsiStr2Unicode((char *) g_arMsgInfo[nMsgID].fields[i].name);

         if(strUnicodeTmp.GetLength() > LEN_FIELDNM - 1)
             strUnicodeTmp = strUnicodeTmp.Left(LEN_FIELDNM - 1);

         _tcscpy(pMavMsg ->arField[i].szFieldNm, strUnicodeTmp);                              // Field Name
         pMavMsg ->arField[i].nFieldTyp = g_arMsgInfo[nMsgID].fields[i].type;             // Field Type

         nOfst = g_arMsgInfo[nMsgID].fields[i].wire_offset;


         // 特殊消息的处理
         if(nMsgID == MAVLINK_MSG_ID_STATUSTEXT && i == 1)
         {
             memset(szStatusTxt, 0x00, sizeof(szStatusTxt));
             memcpy(szStatusTxt, (arMsgBuf + MAVLINK_NUM_HEADER_BYTES + 1), MAVLINK_MSG_ID_STATUSTEXT_LEN);      // 1 为 字符串位置相对载荷开始位置的偏移
             strMsgText = AnsiStr2Unicode(szStatusTxt);
         }
         else if(nMsgID == MAVLINK_MSG_ID_PARAM_VALUE && i == 3)
         {
             memset(szValueID, 0x00, sizeof(szValueID));
             memcpy(szValueID, (arMsgBuf + MAVLINK_NUM_HEADER_BYTES + 8), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
             strMsgText = AnsiStr2Unicode(szValueID);
         }
         else if(nMsgID == MAVLINK_MSG_ID_PARAM_SET && i == 3)
         {
             memcpy(szValueID, (arMsgBuf + MAVLINK_NUM_HEADER_BYTES + 6), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
             strMsgText = AnsiStr2Unicode(szValueID);
         }
         else
         {
             // 一般 mavlink msg 的处理

             // 获取每个数值所存储的缓冲区
             if(g_arMsgInfo[nMsgID].fields[i].type == MAVLINK_TYPE_CHAR ||
                g_arMsgInfo[nMsgID].fields[i].type == MAVLINK_TYPE_UINT8_T ||
                g_arMsgInfo[nMsgID].fields[i].type == MAVLINK_TYPE_INT8_T)
             {
                 pMavMsg ->arField[i].arData[0] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst];
                 ASSERT(nOfst < nLenMaxPayload);
             }
             else if(g_arMsgInfo[nMsgID].fields[i].type == MAVLINK_TYPE_UINT16_T ||
                 g_arMsgInfo[nMsgID].fields[i].type == MAVLINK_TYPE_INT16_T)
             {
                 pMavMsg ->arField[i].arData[0] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst];
                 pMavMsg ->arField[i].arData[1] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 1];

                 ASSERT(nOfst + 1 < nLenMaxPayload);
             }
             else if(g_arMsgInfo[nMsgID].fields[i].type == MAVLINK_TYPE_UINT32_T ||
                 g_arMsgInfo[nMsgID].fields[i].type == MAVLINK_TYPE_INT32_T)
             {
                 pMavMsg ->arField[i].arData[0] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst];
                 pMavMsg ->arField[i].arData[1] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 1];
                 pMavMsg ->arField[i].arData[2] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 2];
                 pMavMsg ->arField[i].arData[3] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 3];

                 ASSERT(nOfst + 3 < nLenMaxPayload);
             }
             else if(g_arMsgInfo[nMsgID].fields[i].type == MAVLINK_TYPE_UINT64_T ||
                 g_arMsgInfo[nMsgID].fields[i].type == MAVLINK_TYPE_INT64_T)
             {
                 pMavMsg ->arField[i].arData[0] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst];
                 pMavMsg ->arField[i].arData[1] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 1];
                 pMavMsg ->arField[i].arData[2] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 2];
                 pMavMsg ->arField[i].arData[3] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 3];
                 pMavMsg ->arField[i].arData[4] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 4];
                 pMavMsg ->arField[i].arData[5] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 5];
                 pMavMsg ->arField[i].arData[6] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 6];
                 pMavMsg ->arField[i].arData[7] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 7];

                 ASSERT(nOfst + 7 < nLenMaxPayload);
             }
             else if(g_arMsgInfo[nMsgID].fields[i].type == MAVLINK_TYPE_FLOAT)           // 4 byte
             {
                 pMavMsg ->arField[i].arData[0] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst];
                 pMavMsg ->arField[i].arData[1] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 1];
                 pMavMsg ->arField[i].arData[2] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 2];
                 pMavMsg ->arField[i].arData[3] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 3];

                 ASSERT(nOfst + 3 < nLenMaxPayload);
             }
             else if(g_arMsgInfo[nMsgID].fields[i].type == MAVLINK_TYPE_DOUBLE)      // 8 byte
             {
                 pMavMsg ->arField[i].arData[0] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst];
                 pMavMsg ->arField[i].arData[1] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 1];
                 pMavMsg ->arField[i].arData[2] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 2];
                 pMavMsg ->arField[i].arData[3] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 3];
                 pMavMsg ->arField[i].arData[4] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 4];
                 pMavMsg ->arField[i].arData[5] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 5];
                 pMavMsg ->arField[i].arData[6] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 6];
                 pMavMsg ->arField[i].arData[7] = arMsgBuf[MAVLINK_NUM_HEADER_BYTES + nOfst + 7];

                 ASSERT(nOfst + 7 < nLenMaxPayload);
             }
             else
             {
                 TRACE(_T("\r\n> MAVLinkProtocol.ParseMsg - Unexpect field type: "));
                 TRACE(_T("\r\n> MAVLinkProtocol.ParseMsg - g_arMsgInfo[nMsgID].fields[i].type = %d"), g_arMsgInfo[nMsgID].fields[i].type);
             }
         }
     }

     return nMsgID;
 }

相关结构的定义：

 struct FIELDSVALUE
 {
     TCHAR   szFieldNm[LEN_FIELDNM];
     uint8_t nFieldTyp;
     BYTE    arData[8];                                      // 字段数值
 };

 struct MSGVALUE
 {
     TCHAR   szMsgName[LEN_MSGNM];
     uint8_t nFieldCnts;

     FIELDSVALUE arField[MAVLINK_MAX_FIELDS];
 };

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

 static mavlink_message_info_t g_arMsgInfo[256] = MAVLINK_MESSAGE_INFO