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)
			strUni