paparazzi message protocol parse

// 根据读取的 messages.xml 初始化msg格式存储结构数组
// xml解析使用了 CMarkup，可参见 http://www.firstobject.com/


BOOL CProtocol::InitTeleMsgDict_PPZ()
{
	// Function    : 根据 messages.xml 文件, 解析获取 ppz msg 的帧格式
	// Remark      : 解析 xml 文件获取的帧格式存储于 m_arMsgPPZ_Tele[] 结构数组当中

	int i, k, nFieldInx;
	BOOL blRet, blTmp;
	BYTE nTmp, nMsgID;
	CString strMsgName, strMsgId, strTmp;

	i = k = 0;
	nFieldInx = 0;
	nMsgID    = 0x00;
	nTmp      = 0x00;
	blRet     = TRUE;
	blTmp    = FALSE;

	#ifdef PROTOCOL_DBG
	TRACE(_T("\r\n>\r\n> CProtocol.InitTeleMsgDict_PPZ"));
	#endif
	
	m_MarkupXml.ResetPos();
	
	if(m_MarkupXml.FindElem(_T("protocol")))
	{
		blTmp = m_MarkupXml.IntoElem();								// to class
		
		for(i = 0; blTmp && i < 20 && m_MarkupXml.FindElem(); i++)	// Enum class
		{
			strTmp = m_MarkupXml.GetAttrib(_T("name"));		// 
			if(strTmp == _T("telemetry"))
			{
				blTmp = m_MarkupXml.IntoElem();						// to message
				
				for(k = 0; blTmp && k < 256 && m_MarkupXml.FindElem(); k++)	// enum msg
				{
					strTmp     = m_MarkupXml.GetTagName();
					strMsgName = m_MarkupXml.GetAttrib(_T("name"));
					strMsgId   = m_MarkupXml.GetAttrib(_T("id"));
					
					nMsgID = _ttoi(strMsgId);
					ASSERT(nMsgID > 0 && nMsgID < 256);

					m_arMsgPPZ_Tele[nMsgID].nMsgID = nMsgID;
					if(strMsgName.GetLength() >= MAX_MSGNM_PPZ - 1)
						strMsgName = strMsgName.Left(MAX_MSGNM_PPZ - 1);
					_tcscpy(m_arMsgPPZ_Tele[nMsgID].szMsgName, strMsgName);
					
					#ifdef PROTOCOL_DBG
					TRACE(_T("\r\n\r\n> Msg: %s (%d)"), strMsgName, nMsgID);
					#endif

					blTmp = m_MarkupXml.IntoElem();				// to field,  

					for(nFieldInx = 0; blTmp && m_MarkupXml.FindElem() && nFieldInx < MAX_FIELDCNTS_PPZ * 2; nFieldInx++)	// Enum field
					{
						if(nFieldInx < MAX_FIELDCNTS_PPZ)
						{
							// field name
							strTmp	= m_MarkupXml.GetAttrib(_T("name"));
							if(strTmp.GetLength() >= MAX_FIELDNM_PPZ - 1)
								strTmp = strTmp.Left(MAX_FIELDNM_PPZ - 1);
							_tcscpy(m_arMsgPPZ_Tele[nMsgID].arFieldInfo[nFieldInx].szFieldNm, strTmp);
							
							// field type
							strTmp = m_MarkupXml.GetAttrib(_T("type"));
							nTmp   = GetFieldTyp_PPZ(strTmp);
							if(nTmp != PPZ_TYPE_UNKNOWN)
							{
								m_arMsgPPZ_Tele[nMsgID].arFieldInfo[nFieldInx].nFieldTyp = nTmp;
							}
							else
							{
								m_strRunInfo.Format(_T("Error: Msg %s.%s: Unknown field type: %s (%d)"), 
													strMsgName, 
													m_arMsgPPZ_Tele[nMsgID].arFieldInfo[nFieldInx].szFieldNm, 
													strTmp, 
													nTmp);
								
								TRACE(_T("\r\n> CProtocol.ParseMsg_PPZ - %s"), m_strRunInfo);
								blRet = FALSE;
							}
								
							// field unit
							strTmp= m_MarkupXml.GetAttrib(_T("alt_unit"));
							if(strTmp.GetLength() >= MAX_UNIT_PPZ - 1)
								strTmp = strTmp.Left(MAX_UNIT_PPZ - 1);
							_tcscpy(m_arMsgPPZ_Tele[nMsgID].arFieldInfo[nFieldInx].szUnit, strTmp);

							// field coef
							strTmp = m_MarkupXml.GetAttrib(_T("alt_unit_coef"));
							m_arMsgPPZ_Tele[nMsgID].arFieldInfo[nFieldInx].dFieldCoef = (float) _tcstod(strTmp, NULL);

							// 
							m_arMsgPPZ_Tele[nMsgID].nFieldCnts++;
							m_arMsgPPZ_Tele[nMsgID].nFrmLen += GetFieldLen_PPZ(m_arMsgPPZ_Tele[nMsgID].arFieldInfo[nFieldInx].nFieldTyp);

						}
						else
						{
							m_strRunInfo.Format(_T("The counts of field overflow"));
							TRACE(_T("\r\n> CProtocol.ParseMsg_PPZ - %s"), m_strRunInfo);
							blRet = FALSE;
						}

						
						#ifdef PROTOCOL_DBG
						::Sleep(5);
						TRACE(_T("\r\n>     - %-16s:  Unit:%-8s, Type:%d  Coef:%f"), 
										m_arMsgPPZ_Tele[nMsgID].arFieldInfo[nFieldInx].szFieldNm, 
										m_arMsgPPZ_Tele[nMsgID].arFieldInfo[nFieldInx].szUnit, 
										m_arMsgPPZ_Tele[nMsgID].arFieldInfo[nFieldInx].nFieldTyp, 
										m_arMsgPPZ_Tele[nMsgID].arFieldInfo[nFieldInx].dFieldCoef);
						#endif
						

					}	// enum field

					#ifdef PROTOCOL_DBG
					TRACE(_T("\r\n> Msg: field: %d  len: %d byte"), m_arMsgPPZ_Tele[nMsgID].nFieldCnts, m_arMsgPPZ_Tele[nMsgID].nFrmLen);
					#endif

					blTmp = m_MarkupXml.OutOfElem();
					ASSERT(blTmp);
				}	// enum msg

				blTmp = m_MarkupXml.OutOfElem();
				ASSERT(blTmp);

				break;
			}
		}

		blTmp = m_MarkupXml.OutOfElem();
		ASSERT(blTmp);
	}
	
	return blRet;
}

BYTE CProtocol::Stream2Frame_PPZ(BYTE byteReceive)
{
	// Function    : 从串口数据流当中解析出 ppz frame
	// Parameters  : byteReceive - 当前接收的字符
	// Return value: 
	// Remark      : 解析成功返回解析出的 ppz msg id, 否则返回 0

	int i;
	BYTE nGotFrmID = 0x00;

	i = 0;
	ASSERT(m_nFrmDatInx < MAX_DLFRMLEN * 2 + 16);

	if(m_nFrmDatInx > MAX_DLFRMLEN * 2)
	{
		RevReset();
		TRACE(_T("\r\n\r\n> CProtocol.Stream2Frame_PPZ - Too error dat. get frame from data stream failed."));
	}


	if(byteReceive == FRAMEHEAD_PPZ && m_blIsFrm == FALSE)			               // 检测 0x99
	{
		#ifdef SERIALDBG
		TRACE(_T("%02X "), m_arTmpRevBuff[m_nCurRevByteInx]);					// for debug
		#endif
					
		m_blIsFrm    = TRUE;
		m_nFrmDatInx = 0;

		m_arTmpRevBuff[m_nFrmDatInx] = byteReceive;
		m_nFrmDatInx++;
	}
	else if(m_blIsFrm)
	{
		// 假设帧头标志匹配, 获取该数据帧的余下数据
		ASSERT(m_nFrmDatInx < MAX_DLFRMLEN);

		m_arTmpRevBuff[m_nFrmDatInx] = byteReceive;

		if(m_nFrmDatInx == PPZINX_LEN)
		{
			m_nPPZFrmLen = byteReceive;
			
			m_byteCk_A = m_byteCk_B = byteReceive;

			m_nInxCkA = m_nPPZFrmLen - 2;
			m_nInxCkB = m_nPPZFrmLen - 1;
			
			#ifdef PROTOCOL_DBG
			TRACE(_T("\r\n> CProtocol.Stream2Frame_PPZ - Frame len = %d, InxCKA = %d InxCKB = %d"), m_nPPZFrmLen, m_nInxCkA, m_nInxCkB);
			#endif
		}
		else if(m_nFrmDatInx == PPZINX_SENDERID)
		{
			m_nSenderID = byteReceive;
			
			m_byteCk_A += byteReceive;
			m_byteCk_B += m_byteCk_A;

			#ifdef PROTOCOL_DBG
			TRACE(_T("\r\n> CProtocol.Stream2Frame_PPZ - Sender ID = %d"), m_nSenderID);
			#endif
		}
		else if(m_nFrmDatInx == PPZINX_MSGID)
		{
			m_nMsgID = byteReceive;

			m_byteCk_A += byteReceive;
			m_byteCk_B += m_byteCk_A;

			#ifdef PROTOCOL_DBG
			TRACE(_T("\r\n> CProtocol.Stream2Frame_PPZ - Msg ID = %d (0x%02X)"), m_nMsgID, m_nMsgID);
			#endif
		}
		else if(m_nFrmDatInx < m_nInxCkA)
		{
			m_byteCk_A += byteReceive;
			m_byteCk_B += m_byteCk_A;
		}

		if(m_nFrmDatInx >= m_nInxCkB)
		{	// 假设当前已经接收到了一个完整帧

			ASSERT(m_nInxCkA < MAX_DLFRMLEN * 2 + 16 && m_nInxCkB < MAX_DLFRMLEN * 2 + 16);
			ASSERT(m_nInxCkA > -1 && m_nInxCkB > -1);

			if(m_arTmpRevBuff[m_nInxCkA] == m_byteCk_A && m_arTmpRevBuff[m_nInxCkB] == m_byteCk_B)
			{
				ASSERT(m_nPPZFrmLen > 0 && m_nPPZFrmLen <= MAX_DLFRMLEN);

				memset(m_arRevFrameBuff, 0x00, sizeof(m_arRevFrameBuff));
				memcpy(m_arRevFrameBuff, m_arTmpRevBuff, m_nPPZFrmLen);
				
				nGotFrmID = m_arRevFrameBuff[3];

				#ifdef PROTOCOL_DBG
				TRACE(_T("\r\n> CProtocol.Stream2Frame_PPZ - Rev frame:  "));
				for(i = 0; i < m_nPPZFrmLen; i++)
					TRACE(_T("%02X "), m_arRevFrameBuff[i]);
				TRACE(_T("\r\n> "));
				#endif
			}

			RevReset();
		}

		m_nFrmDatInx++;
	}

	return nGotFrmID;
}


void CProtocol::RevReset()
{
	memset(m_arTmpRevBuff, 0x00, sizeof(m_arTmpRevBuff));
	
	m_blIsFrm	 = FALSE;
	m_nFrmDatInx = 0;
	
	m_nInxCkA    = -1;
	m_nInxCkB    = -1;  
	
	m_byteCk_A   = 0x00;
	m_byteCk_B   = 0x00;
}


BYTE CProtocol::ParseTeleMsg_PPZ(BYTE nMsgID)
{
	// Function    : 从接收到的 ppz 数据帧当中将 telemetry class 当中帧字段解析出来并存放于 m_arMsgPPZ_Tele 当中
	// Parameters  : 
	// Return value: 解析成功返回当前解析的消息 nMsgID 当中所包含的 field 数量 
	//               若解析失败或解析的 msg 不属于 class telemetry, 则返回 0

	int i, nFieldCnts, nPayloadLen;
	BYTE nParseFields;
	BYTE nFieldLen, nFieldTyp;
	BYTE arTemp[8];
	
	BOOL blNoArray;

	BYTE *pPayload;
	BYTE *pCurFieldBuff;

	blNoArray     = TRUE;

	nPayloadLen   = 0;
	nParseFields  = 0;

	pPayload      = NULL;
	pCurFieldBuff = NULL;

	nFieldLen = nFieldTyp = 0;
	memset(arTemp, 0x00, sizeof(arTemp));

	if(nMsgID < 0x01)
		return nParseFields;

	ASSERT(nMsgID < MAX_MSGCNTS_PPZ);
	nFieldCnts = m_arMsgPPZ_Tele[nMsgID].nFieldCnts;
	ASSERT(nFieldCnts < MAX_FIELDCNTS_PPZ);

	pPayload      = m_arRevFrameBuff + OFFSET_PAYLOAD;
	pCurFieldBuff = pPayload;

	for(i = 0; i < nFieldCnts; i++)
	{
		nFieldTyp = m_arMsgPPZ_Tele[nMsgID].arFieldInfo[i].nFieldTyp;
		nFieldLen = GetFieldLen_PPZ(nFieldTyp);

		if(nFieldLen > 0)
		{
			memset(arTemp, 0x00, sizeof(arTemp));

			ASSERT(nFieldLen <= 8);
			ASSERT(pCurFieldBuff);

			memcpy(arTemp, pCurFieldBuff, nFieldLen);
			m_arMsgPPZ_Tele[nMsgID].arFieldInfo[i].dFieldValue = GetFieldValue_PPZ(nFieldTyp, arTemp);

			pCurFieldBuff += nFieldLen;			// 字段缓冲区指针
			nPayloadLen   += nFieldLen;

			ASSERT(nPayloadLen <= m_nPPZFrmLen - LEN_NONEPAYLOAD);
		}
		else
		{
			blNoArray = FALSE;

			if(nFieldTyp == PPZ_TYPE_UINT8_ARRAY)
				;
			else if(nFieldTyp == PPZ_TYPE_UINT16_ARRAY)
				;
			else if(nFieldTyp == PPZ_TYPE_INT16_ARRAY)
				;
			else if(nFieldTyp == PPZ_TYPE_UINT32_ARRAY)
				;
		}

		nParseFields++;

	}

	if(blNoArray)
		if(nPayloadLen + LEN_NONEPAYLOAD != m_nPPZFrmLen)
			nParseFields = 0;

	return nParseFields;
}

double CProtocol::GetFieldValue_PPZ(int nFieldTyp, BYTE arData[])
{
	double dRet = 0.0;

	uint8_t		uDat_1;
	uint16_t	uDat_2;
	uint32_t	uDat_4;
	uint64_t	uDat_8;
	
	int8_t		Dat_1;
	int16_t		Dat_2;
	int32_t		Dat_4;
	int64_t		Dat_8;
	
	float		fDat;
	double		dDat;

	uDat_1 = 0;
	uDat_2 = 0;
	uDat_4 = 0;
	uDat_8 = 0;
	
	Dat_1  = 0;
	Dat_2  = 0;
	Dat_4  = 0;
	Dat_8  = 0;
	
	fDat   = 0.0;
	dDat   = 0.0;

	if(nFieldTyp == PPZ_TYPE_CHAR || nFieldTyp == PPZ_TYPE_UINT8_T)
	{
		memcpy(&uDat_1, arData, 1);
		dRet = (double) uDat_1;
	}
	else if(nFieldTyp == PPZ_TYPE_INT8_T)
	{
		memcpy(&Dat_1, arData, 1);
		dRet = (double) Dat_1;
	}
	else if(nFieldTyp == PPZ_TYPE_UINT16_T)
	{
		memcpy(&uDat_2, arData, 2);
		dRet = (double) uDat_2;
	}
	else if(nFieldTyp == PPZ_TYPE_INT16_T)
	{
		memcpy(&Dat_2, arData, 2);
		dRet = (double) Dat_2;
	}
	else if(nFieldTyp == PPZ_TYPE_UINT32_T)
	{
		memcpy(&uDat_4, arData, 4);
		dRet = (double) uDat_4;
	}
	else if(nFieldTyp == PPZ_TYPE_INT32_T)
	{
		memcpy(&Dat_4, arData, 4);
		dRet = (double) Dat_4;
	}
	else if(nFieldTyp == PPZ_TYPE_UINT64_T)
	{
		// 此处一定要注意, dRet 并不是变量的真正数值, 而是容纳变量的一个容器
		memcpy(&uDat_8, arData, 8);
		memcpy(&dRet, arData, 8);
	}
	else if(nFieldTyp == PPZ_TYPE_INT64_T)
	{	// 此处一定要注意, dRet 并不是变量的真正数值, 而是容纳变量的一个容器
		memcpy(&Dat_8, arData, 8);
		dRet = (double) Dat_8;
	}
	else if(nFieldTyp == PPZ_TYPE_FLOAT)
	{
		memcpy(&fDat, arData, 4);
		dRet = (double) fDat;
	}
	else if(nFieldTyp == PPZ_TYPE_DOUBLE)
	{
		memcpy(&dDat, arData, 8);
		dRet = dDat;
	}

	return dRet;
}

相关结构定义：

// user define data
struct FIELD_PPZ
{
	TCHAR	szFieldNm[MAX_FIELDNM_PPZ];
	TCHAR	szUnit[MAX_UNIT_PPZ];
	BYTE    nFieldTyp;
	float	dFieldCoef;

	double  dFieldValue;
};

struct MSG_PPZ
{
	TCHAR	szMsgName[MAX_MSGNM_PPZ];
	BYTE	nMsgID;
	BYTE    nFieldCnts;
	BYTE    nFrmLen;
	
	FIELD_PPZ arFieldInfo[MAX_FIELDCNTS_PPZ];
};

static MSG_PPZ	m_arMsgPPZ_Tele[MAX_MSGCNTS_PPZ];