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