diff --git a/df_datalink_sp/src/networking/link_layer/msg_def.h b/df_datalink_sp/src/networking/link_layer/msg_def.h index e22067d..93c0184 100644 --- a/df_datalink_sp/src/networking/link_layer/msg_def.h +++ b/df_datalink_sp/src/networking/link_layer/msg_def.h @@ -79,6 +79,10 @@ #define L2_MSG_CFM_SRC_SPEEDY_LEN 4 #define L2_MSG_CFM_SRC_SPEEDZ 28 #define L2_MSG_CFM_SRC_SPEEDZ_LEN 4 +#define L2_MSG_CFM_PAYLOAD_ID 32 +#define L2_MSG_CFM_PAYLOAD_ID_LEN 4 +#define L2_MSG_CFM_PAYLOAD 36 +#define L2_MSG_CFM_PAYLOAD_LEN 230 #define L2_MSG_CFM_CRC 266 #define L2_MSG_CFM_CRC_LEN 2 diff --git a/df_datalink_sp/src/networking/link_layer/scan.c b/df_datalink_sp/src/networking/link_layer/scan.c index d7bfcef..41dfc39 100644 --- a/df_datalink_sp/src/networking/link_layer/scan.c +++ b/df_datalink_sp/src/networking/link_layer/scan.c @@ -360,6 +360,23 @@ static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dst L2_FrameInsertChunk(buff, L2_MSG_CFM_SRC_SPEEDY, L2_MSG_CFM_SRC_SPEEDY_LEN, &(instPtr->sysInfo.vY)); L2_FrameInsertChunk(buff, L2_MSG_CFM_SRC_SPEEDZ, L2_MSG_CFM_SRC_SPEEDZ_LEN, &(instPtr->sysInfo.vZ)); + // Packing extra payload + uint32_t dataId; + if(L2_GetNodeType(instPtr) == L2_NODE_HEAD) + { + SysMonitor_GetSendPayload(buff + L2_MSG_CFM_PAYLOAD, &dataId, PAYLOAD_LEN_FLJ_TX); + } + else + { + SysMonitor_GetSendPayload(buff + L2_MSG_CFM_PAYLOAD, &dataId, PAYLOAD_LEN_LJ_TX); + } + + L2_FrameInsertChunk(buff, L2_MSG_CFM_PAYLOAD_ID, L2_MSG_CFM_PAYLOAD_ID_LEN, &dataId); + if (dataId == DATA_ID_NONE) + { + memset(buff + L2_MSG_CFM_PAYLOAD, 0xAA, L2_MSG_CFM_PAYLOAD_LEN); + } + // Append CRC Status = L2_CrcAppend(buff, L2_CMF_PACK_LEN - L2_MSG_CFM_CRC_LEN, L2_MSG_CFM_CRC, L2_CRC16_1021); if (Status != SUCCESS) @@ -637,6 +654,23 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) // Time sync. L2_TimeAdjust(instPtr, &(instPtr->log.timeDev)); } + + // Get data payload + uint32_t dataId; + L2_FrameGetChunk(msg, L2_MSG_CFM_PAYLOAD_ID, L2_MSG_CFM_PAYLOAD_ID_LEN, &dataId); + + if(dataId != DATA_ID_NONE) + { + if(L2_GetNodeType(instPtr) == L2_NODE_HEAD) + { + SysMonitor_UpdateRecvPayload(msg+L2_MSG_CFM_PAYLOAD, PAYLOAD_LEN_FLJ_RX); + } + else + { + SysMonitor_UpdateRecvPayload(msg+L2_MSG_CFM_PAYLOAD, PAYLOAD_LEN_LJ_RX); + } + } + Status = SUCCESS; break; }