From 169cb6bd919966fba7512f422f4ec386dd0dc915 Mon Sep 17 00:00:00 2001 From: Josh Gao Date: Wed, 21 May 2025 17:11:38 +0800 Subject: [PATCH] =?UTF-8?q?fix:=20=E4=BF=AE=E6=AD=A3=E5=BB=BA=E7=BD=91?= =?UTF-8?q?=E6=95=B0=E6=8D=AE=E6=89=93=E5=8C=85,=20=E5=A2=9E=E5=8A=A0routi?= =?UTF-8?q?ne=E5=A4=84=E7=90=86=E5=88=86=E6=94=AF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../src/networking/link_layer/scan.c | 119 +++++++++--------- .../src/networking/phy_layer/phy_layer.c | 4 +- .../src/networking/phy_layer/phy_layer.h | 2 +- 3 files changed, 65 insertions(+), 60 deletions(-) diff --git a/df_datalink_sp/src/networking/link_layer/scan.c b/df_datalink_sp/src/networking/link_layer/scan.c index c403a92..a2853fe 100644 --- a/df_datalink_sp/src/networking/link_layer/scan.c +++ b/df_datalink_sp/src/networking/link_layer/scan.c @@ -56,18 +56,11 @@ static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dst { ASSERT(instPtr != NULL); int32_t Status; - uint32_t dataChunk; - if ((type < L2_FRAME_ADV) || (type > L2_FRAME_ACK)) - { - DBG_ERROR("Unexpected frame type: %" PRId8 ".\n", type); - return EPARAM; - } switch (type) { case L2_FRAME_ADV: - { - L2_MSG_ADV *message = calloc(sizeof(L2_MSG_ADV)); + L2_MSG_ADV *message = (L2_MSG_ADV *)(instPtr->sendPktData); // Common section of link layer package message->common->frameNum = L2_GetNextFrameNum(instPtr); @@ -78,41 +71,37 @@ static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dst // insert position and speed info SysMonitor_GetSysinfo(&(instPtr->sysInfo)); - message->srcPosX = &(instPtr->sysInfo.posX); - message->srcPosY = &(instPtr->sysInfo.posY); - message->srcPosZ = &(instPtr->sysInfo.posZ); + message->srcPosX = instPtr->sysInfo.posX; + message->srcPosY = instPtr->sysInfo.posY; + message->srcPosZ = instPtr->sysInfo.posZ; // Insert net start time message->srcNetStatTime = &(instPtr->netStartTime); // Packing extra payload SysMonitor_GetSendPayload(message->advData, message->advDataId, L2_PAYLOAD_LEN_ADV); - if (message->ackDataId == DATA_ID_NONE) + if (message->advDataId == DATA_ID_NONE) { - memset(message->ackData, 0xAA, L2_PAYLOAD_LEN_ADV); + memset(message->advData, 0xAA, L2_PAYLOAD_LEN_ADV); } // Append CRC uint32_t crcVal; - Status = L2_CrcGen(message, Phy_GetFrameLen(L2_L2frame2L1frame(L2_FRAME_ADV)) - L2_MSG_CRC_LEN, L2_CRC8_31, &crcVal); + Status = L2_CrcGen(message, sizeof(L2_MSG_ADV) - L2_MSG_CRC_LEN, L2_CRC8_31, &crcVal); if (Status != SUCCESS) { + DBG_ERROR("Fail to generate CRC, error code: %" PRId32 ".\n", Status); return Status; } message->crc = crcVal; - instPtr->sendPktData = message; - // Logging... - memcpy(instPtr->log.txLog.data, message, Phy_GetFrameLen(L2_L2frame2L1frame(L2_FRAME_ADV))); - instPtr->log.txLog.dataId = message->ackDataId; + memcpy(instPtr->log.txLog.data, message, sizeof(L2_MSG_ADV)); + instPtr->log.txLog.dataId = message->advDataId; - free(message); break; - } case L2_FRAME_ACK: - { - L2_MSG_ACK *message = calloc(sizeof(L2_MSG_ACK)); + L2_MSG_ACK *message = (L2_MSG_ACK *)(instPtr->sendPktData); // Common section of link layer package message->common->frameNum = L2_GetNextFrameNum(instPtr); @@ -123,12 +112,12 @@ static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dst // insert position and speed info SysMonitor_GetSysinfo(&(instPtr->sysInfo)); - message->srcPosX = &(instPtr->sysInfo.posX); - message->srcPosY = &(instPtr->sysInfo.posY); - message->srcPosZ = &(instPtr->sysInfo.posZ); - message->srcSpeedX = &(instPtr->&(instPtr->sysInfo.vX)); - message->srcSpeedY = &(instPtr->&(instPtr->sysInfo.vY)); - message->srcSpeedZ = &(instPtr->&(instPtr->sysInfo.vZ)); + message->srcPosX = instPtr->sysInfo.posX; + message->srcPosY = instPtr->sysInfo.posY; + message->srcPosZ = instPtr->sysInfo.posZ; + message->srcSpeedX = instPtr->sysInfo.vX; + message->srcSpeedY = instPtr->sysInfo.vY; + message->srcSpeedZ = instPtr->sysInfo.vZ; // Packing extra payload SysMonitor_GetSendPayload(message->ackData, message->ackDataId, L2_PAYLOAD_LEN_ACK); @@ -139,26 +128,23 @@ static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dst // Append CRC uint32_t crcVal; - Status = L2_CrcGen(message, Phy_GetFrameLen(L2_L2frame2L1frame(L2_FRAME_ACK)) - L2_MSG_CRC_LEN, L2_CRC8_31, &crcVal); + Status = L2_CrcGen(message, sizeof(L2_MSG_ACK) - L2_MSG_CRC_LEN, L2_CRC8_31, &crcVal); if (Status != SUCCESS) { + DBG_ERROR("Fail to generate CRC, error code: %" PRId32 ".\n", Status); return Status; } message->crc = crcVal; // Logging... - memcpy(instPtr->log.txLog.data, message, Phy_GetFrameLen(L2_L2frame2L1frame(L2_FRAME_ACK))); + memcpy(instPtr->log.txLog.data, message, sizeof(L2_MSG_ACK)); instPtr->log.txLog.dataId = message->ackDataId; - free(message); break; - } default: - { DBG_ERROR("Unexpected frame type: %" PRId8 ".\n", type); return EPARAM; } - } return SUCCESS; } @@ -191,7 +177,7 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) break; } - L2_MSG_ADV *message = msg; + L2_MSG_ADV *message = (L2_MSG_ADV *)msg; // Update node status. uint8_t nodeId = message->common.src; @@ -207,11 +193,12 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) break; } } + // Update node info. node->nodeInfo.pos.x = message->srcPosX; node->nodeInfo.pos.y = message->srcPosY; node->nodeInfo.pos.z = message->srcPosZ; - node->nodeInfo.antIndex = Phy_ListenAntIndex(instPtr->config.phyInstPtr); + node->nodeInfo.antIndex = Phy_ReceiveAntIndex(instPtr->config.phyInstPtr); // Reset node ttl node->ttlCnt = 0; @@ -253,7 +240,7 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) SysMonitor_UpdateRecvPayload(message->advData, dataId, L2_PAYLOAD_LEN_ADV); // Logging... - memcpy(instPtr->log.rxLog.data, msg, Phy_GetFrameLen(L2_L2frame2L1frame(L2_FRAME_ADV, L2_SVC_NONE))); + memcpy(instPtr->log.rxLog.data, msg, sizeof(L2_MSG_ADV)); instPtr->log.rxLog.dataId = dataId; Status = SUCCESS; @@ -268,7 +255,7 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) break; } - L2_MSG_ACK *message = msg; + L2_MSG_ACK *message = (L2_MSG_ACK *)msg; // Update node status. uint8_t nodeId = message->common.src; @@ -284,6 +271,7 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) break; } } + // Update node info. node->nodeInfo.pos.x = message->srcPosX; node->nodeInfo.pos.y = message->srcPosY; @@ -291,7 +279,7 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) node->nodeInfo.speed.x = message->srcSpeedX; node->nodeInfo.speed.y = message->srcSpeedY; node->nodeInfo.speed.z = message->srcSpeedZ; - node->nodeInfo.antIndex = Phy_ListenAntIndex(instPtr->config.phyInstPtr); + node->nodeInfo.antIndex = Phy_ReceiveAntIndex(instPtr->config.phyInstPtr); // Reset node ttl node->ttlCnt = 0; @@ -321,7 +309,7 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) SysMonitor_UpdateRecvPayload(message->advData, dataId, L2_PAYLOAD_LEN_ADV); // Logging... - memcpy(instPtr->log.rxLog.data, msg, Phy_GetFrameLen(L2_L2frame2L1frame(L2_FRAME_ACK, L2_SVC_NONE))); + memcpy(instPtr->log.rxLog.data, msg, sizeof(L2_MSG_ACK)); instPtr->log.rxLog.dataId = dataId; Status = SUCCESS; @@ -381,10 +369,6 @@ void L2_Scan_Destroy(L2_SCAN_FSM *scanFSMPtr) int32_t L2_Scan_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntry) { ASSERT(instPtr != NULL); - L2_TOR_TYPE torState; - const NODE_ENTRY *commNode; - PHY_ANT_INDEX antIndex; - PHY_INST *phyRef = instPtr->config.phyInstPtr; DBG_INFO("-------- Scan Routine --------\n"); if (rtneEntry->slotSeq != 0) @@ -392,14 +376,24 @@ int32_t L2_Scan_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntry) return SUCCESS; // skip other slots, only process in the 1st slot } + PHY_INST *phyRef = instPtr->config.phyInstPtr; + PHY_ANT_INDEX antIndex; + L2_TOR_TYPE torState; + const NODE_ENTRY *commNode; + switch (rtneEntry->slotType) { case L2_SLOT_ADVERTISE: if (L2_GetNodeType(instPtr) == L2_NODE_HEAD && rtneEntry->src == L2_GetNodeId(instPtr)) { - antIndex = L2_AntId2AntIndex(rtneEntry->dst); // antenna selection based on broadcast id in routing table - torState = L2_TOR_TX; // head node transmits messages in advertise slot + // antenna selection based on broadcast id in routing table + antIndex = L2_AntId2AntIndex(rtneEntry->dst); + // head node transmits messages in advertise slot + torState = L2_TOR_TX; + // reset memory + memset(instPtr->sendPktData, 0xAA, L2_PACK_LEN_MAX); + // pack message if (ScanMsg_Packing(instPtr, L2_FRAME_ADV, rtneEntry->dst) != SUCCESS) { return FAILURE; @@ -408,13 +402,16 @@ int32_t L2_Scan_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntry) else if (L2_GetNodeType(instPtr) == L2_NODE_MEMBER) { commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->src); - antIndex = commNode->nodeInfo.antIndex; // antenna selection based on node list - torState = L2_TOR_RX; // member node receives message in advertise slot + // antenna selection based on node list + antIndex = commNode->nodeInfo.antIndex; + // member node receives message in advertise slot + torState = L2_TOR_RX; } else { - DBG_ERROR("Unsupported node type" PRId8 "or wrong routine table in L2_SLOT_ADVERTISE.\n", L2_GetNodeType(instPtr)); - return FAILURE; + // otherwise suspend + antIndex = PHY_ANT_INDEX_NONE; + torState = L2_TOR_SUSPEND; } (void)Phy_SetAntIndex(instPtr->config.phyInstPtr, antIndex); (void)L2_SetupNextSlot(instPtr, torState, L2_FRAME_ADV); @@ -422,15 +419,22 @@ int32_t L2_Scan_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntry) case L2_SLOT_ACKNOWLEDGE: if (L2_GetNodeType(instPtr) == L2_NODE_HEAD && rtneEntry->dst == L2_GetNodeId(instPtr)) { - antIndex = PHY_ANT_INDEX_NONE; // FIXME: is antenna index necessary here? - torState = L2_TOR_RX; // head node receives messages in acknowledge slot + // FIXME: is antenna index necessary here? + antIndex = PHY_ANT_INDEX_NONE; + // head node receives messages in acknowledge slot + torState = L2_TOR_RX; } - else if (L2_GetNodeType(instPtr) == L2_NODE_MEMBER) + else if (L2_GetNodeType(instPtr) == L2_NODE_MEMBER && rtneEntry->src == L2_GetNodeId(instPtr)) { commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->src); - antIndex = commNode->nodeInfo.antIndex; // antenna selection based on node list - torState = L2_TOR_TX; // member node transmits message in acknowledge slot + // antenna selection based on node list + antIndex = commNode->nodeInfo.antIndex; + // member node transmits message in acknowledge slot + torState = L2_TOR_TX; + // reset memory + memset(instPtr->sendPktData, 0xAA, L2_PACK_LEN_MAX); + // pack message if (ScanMsg_Packing(instPtr, L2_FRAME_ACK, rtneEntry->dst) != SUCCESS) { return FAILURE; @@ -438,8 +442,9 @@ int32_t L2_Scan_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntry) } else { - DBG_ERROR("Unsupported node type" PRId8 "or wrong routine table in L2_SLOT_ACKNOWLEDGE.\n", L2_GetNodeType(instPtr)); - return FAILURE; + // otherwise suspend + antIndex = PHY_ANT_INDEX_NONE; + torState = L2_TOR_SUSPEND; } (void)Phy_SetAntIndex(instPtr->config.phyInstPtr, antIndex); (void)L2_SetupNextSlot(instPtr, torState, L2_FRAME_ACK); diff --git a/df_datalink_sp/src/networking/phy_layer/phy_layer.c b/df_datalink_sp/src/networking/phy_layer/phy_layer.c index 36ad869..899e503 100644 --- a/df_datalink_sp/src/networking/phy_layer/phy_layer.c +++ b/df_datalink_sp/src/networking/phy_layer/phy_layer.c @@ -1058,14 +1058,14 @@ void Phy_RefreshHwSate(PHY_INST *instPtr) */ /*****************************************************************************/ /** - * @brief Get the antenna number after physical listening. + * @brief Get the antenna number after physical receive. * * @param instPtr a pointer to physical instance * * @return antenna index * ******************************************************************************/ -uint8_t Phy_ListenAntIndex(PHY_INST *instPtr) +uint8_t Phy_ReceiveAntIndex(PHY_INST *instPtr) { ASSERT(instPtr != NULL); return (uint8_t)PhyRead(instPtr, ANT_SELECT_INDEX); diff --git a/df_datalink_sp/src/networking/phy_layer/phy_layer.h b/df_datalink_sp/src/networking/phy_layer/phy_layer.h index 28ea689..9a24d44 100644 --- a/df_datalink_sp/src/networking/phy_layer/phy_layer.h +++ b/df_datalink_sp/src/networking/phy_layer/phy_layer.h @@ -265,7 +265,7 @@ L1_FRAME_TYPE Phy_GetLastSendFrameType(PHY_INST *instPtr); * @defgroup PhysicalLayer_Receive_Estimations * @{ */ -uint8_t Phy_ListenAntIndex(PHY_INST *instPtr); +uint8_t Phy_ReceiveAntIndex(PHY_INST *instPtr); double Phy_GetFreqEst(PHY_INST *instPtr); double Phy_GetCoarseFreqEst(PHY_INST *instPtr); double Phy_GetFineFreqEst(PHY_INST *instPtr);