Mavlink 协议硬解析主要代码

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