From 6b5ab3dc5995788f0d22e764b1319a3fbade0217 Mon Sep 17 00:00:00 2001 From: Josh Gao Date: Mon, 19 May 2025 16:29:26 +0800 Subject: [PATCH] =?UTF-8?q?feat:=20=E5=AE=8C=E6=88=90=E6=89=AB=E6=8F=8F?= =?UTF-8?q?=E5=BB=BA=E7=BD=91=E5=A4=A7=E6=A1=86=E6=9E=B6=E4=BF=AE=E6=94=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- df_datalink_ge/src/main.c | 2 +- .../src/networking/link_layer/link_layer.c | 26 +- .../src/networking/link_layer/link_layer.h | 1 + .../networking/link_layer/link_layer_types.h | 37 +- .../src/networking/link_layer/node_list.c | 2 +- .../src/networking/link_layer/scan.c | 636 ++++-------------- df_datalink_sp/src/system_monitor.h | 2 +- 7 files changed, 184 insertions(+), 522 deletions(-) diff --git a/df_datalink_ge/src/main.c b/df_datalink_ge/src/main.c index 2a1c8f3..c14754e 100644 --- a/df_datalink_ge/src/main.c +++ b/df_datalink_ge/src/main.c @@ -341,7 +341,7 @@ void Phy_IntrHandler(void *context) /****************************************************************************/ /** - * Handler of periodic 5ms interrupt + * Handler of periodic 2 ms interrupt * * @param callBackRef is the linklayer instance pointer. * diff --git a/df_datalink_sp/src/networking/link_layer/link_layer.c b/df_datalink_sp/src/networking/link_layer/link_layer.c index e500576..129f0fe 100644 --- a/df_datalink_sp/src/networking/link_layer/link_layer.c +++ b/df_datalink_sp/src/networking/link_layer/link_layer.c @@ -439,6 +439,30 @@ L1_FRAME_TYPE L2_L2frame2L1frame(L2_FRAME_TYPE l2Frame, L2_SVC_RATE l2SvcRate) } } +/*****************************************************************************/ +/** + * @brief Convert antenna broadcast id to antenna index + * + * @param antId + * + * @return Physical antenna index. + * + ******************************************************************************/ +PHY_ANT_INDEX L2_AntId2AntIndex(uint8_t antId) +{ + switch (antId) + { + case L2_BROADCAST_ID1: + return PHY_ANT_INDEX_1; + case L2_BROADCAST_ID2: + return PHY_ANT_INDEX_2; + case L2_BROADCAST_ID3: + return PHY_ANT_INDEX_3; + default: + return PHY_ANT_INDEX_NONE; + } +} + /** @} LinkLayer_Utility*/ /** @@ -682,7 +706,7 @@ void Linklayer_Destroy(L2_INST *instPtr) int32_t Linklayer_Start(L2_INST *instPtr) { ASSERT(instPtr != NULL); - // Set netStartTime at first call. + // Set netStartTime at first call, i.e. at the beginning of next time unit. if (instPtr->netStartTime == NET_START_TIME_INFTY) { instPtr->netStartTime = TimeCtrl_GetIntCnt(instPtr->config.tmcInstPtr) / L2_SLOTS_PER_FRAME + 1; diff --git a/df_datalink_sp/src/networking/link_layer/link_layer.h b/df_datalink_sp/src/networking/link_layer/link_layer.h index 5767b1c..c8dcd0f 100644 --- a/df_datalink_sp/src/networking/link_layer/link_layer.h +++ b/df_datalink_sp/src/networking/link_layer/link_layer.h @@ -226,6 +226,7 @@ uint32_t L2_GetNextSlotNum(L2_INST *instPtr); L2_FRAME_TYPE L2_GetNextFrameType(L2_INST *instPtr); L2_TOR_TYPE L2_GetNextTor(L2_INST *instPtr); L1_FRAME_TYPE L2_L2frame2L1frame(L2_FRAME_TYPE l2Frame, L2_SVC_RATE l2SvcRate); +PHY_ANT_INDEX L2_AntId2AntIndex(uint8_t antId); /** @} LinkLayer_Utility*/ #endif /* __LINK_LAYER_H_ */ diff --git a/df_datalink_sp/src/networking/link_layer/link_layer_types.h b/df_datalink_sp/src/networking/link_layer/link_layer_types.h index 3aae004..06de8fd 100644 --- a/df_datalink_sp/src/networking/link_layer/link_layer_types.h +++ b/df_datalink_sp/src/networking/link_layer/link_layer_types.h @@ -33,7 +33,9 @@ * @{ */ // Common node ID definition -#define L2_BROADCAST_ID 0xFF +#define L2_BROADCAST_ID1 0xF1 // broadcast +#define L2_BROADCAST_ID2 0xF2 +#define L2_BROADCAST_ID3 0xF4 #define L2_NONE_ID 0 // Routine configure @@ -45,7 +47,7 @@ #define L2_FRAMENO_NONE 0 // Common number of slots definition -#define L2_SLOTS_PER_FRAME 500 // 2 ms per slot, 1 s per time unit (frame) +#define L2_SLOTS_PER_FRAME 250 // 2 ms per slot, 250 slots per time unit /** @} Linklayer_Common_Constants*/ @@ -106,7 +108,7 @@ typedef struct uint8_t clustId; ///< node cluster ID uint8_t headId; ///< cluster head ID L2_NODE_TYPE nodeType; ///< node type - uint8_t antNo; ///< antenna number to this node + uint8_t antIndex; ///< antenna number to this node COORD pos; ///< node position COORD speed; ///< node speed QUATERNION attitude; ///< node attitude @@ -142,7 +144,7 @@ typedef struct } L2_EVENT; /** - * @enum L2_SLOT_INTER_TYPE + * @enum L2_SLOT_TYPE */ typedef enum { @@ -154,22 +156,7 @@ typedef enum L2_SLOT_MANAGE = 5, L2_SLOT_RTT = 6, L2_SLOT_SERVICE = 7 -} L2_SLOT_INTER_TYPE; - -/** - * @enum L2_SLOT_INNER_TYPE - */ -typedef enum -{ - L2_SLOT_NONE = 0, - L2_SLOT_IDLE = 1, - L2_SLOT_ADVERTISE = 2, - L2_SLOT_ACKNOWLEDGE = 3, - L2_SLOT_DISTRIBUTE = 4, - L2_SLOT_MANAGE = 5, - L2_SLOT_RTT = 6, - L2_SLOT_SERVICE = 7 -} L2_SLOT_INNER_TYPE; +} L2_SLOT_TYPE; /** * @enum L2_FRAME_TYPE @@ -225,8 +212,7 @@ typedef struct typedef enum { L2_SCAN_STAGE_NONE = 0, - L2_SCAN_STAGE_ADV = 1, - L2_SCAN_STAGE_ACK = 2 + L2_SCAN_STAGE_ADV = 1 } SCAN_STAGE; /** @@ -236,13 +222,8 @@ typedef enum typedef struct { bool isAdvRecvd; ///< indicates if advertisement frame done(antenna locked). - bool isAckPending; ///< indicates if confirm message acked - uint8_t scanCnt; ///< scan count - uint8_t advTimeout; ///< adv receive try count - uint8_t ackTimeout; ///< ack receive try count - SCAN_STAGE scanStage; ///< scan stage, only for member node + uint8_t advCnt; ///< adv receive try count uint8_t antIndex; ///< target antenna index - uint8_t tableNo; ///< scan table index } L2_SCAN_FSM; /** diff --git a/df_datalink_sp/src/networking/link_layer/node_list.c b/df_datalink_sp/src/networking/link_layer/node_list.c index 7f81c5b..d156dc7 100644 --- a/df_datalink_sp/src/networking/link_layer/node_list.c +++ b/df_datalink_sp/src/networking/link_layer/node_list.c @@ -198,7 +198,7 @@ NODE_ENTRY *L2_AppendNode(NODE_LIST *list, uint8_t nodeId, L2_NODE_TYPE nodeType nodeEntry->nodeInfo.speed.x = 0; nodeEntry->nodeInfo.speed.y = 0; nodeEntry->nodeInfo.speed.z = 0; - nodeEntry->nodeInfo.beamNo = 0; + nodeEntry->nodeInfo.antIndex = 0; nodeEntry->ttlCnt = 0; clist_rpush(list, node); return nodeEntry; diff --git a/df_datalink_sp/src/networking/link_layer/scan.c b/df_datalink_sp/src/networking/link_layer/scan.c index f164bee..2894642 100644 --- a/df_datalink_sp/src/networking/link_layer/scan.c +++ b/df_datalink_sp/src/networking/link_layer/scan.c @@ -31,122 +31,20 @@ #include "../phy_layer/phy_layer.h" /************************** Constant Definitions *****************************/ -#define ADV_LISTEN_TIMEOUT 40 -#define CFM_LISTEN_TIMEOUT 5 -#define ADV_FRAME_SLOTS 4 -#define CFM_FRAME_SLOTS 5 +#define ACK_FRAME_SLOTS 4 /***************** Macros (In-line Functions) Definitions *********************/ /**************************** Type Definitions *******************************/ /************************** Variable Definitions *****************************/ -// Pair by regex -// Regex: ((\{\.oaxis = [0-9]+[ ,]+\.raxis = [0-9]+[ ]*\}, ){2}) -PHY_ANT_ANGLE antNrrwPattern_0[L1_ANT_PATTERN_NUM] = -{ - {.oaxis = 0 , .raxis = 0 }, - {.oaxis = 139, .raxis = 2225}, - {.oaxis = 152, .raxis = 850 }, - {.oaxis = 138, .raxis = 3155}, - {.oaxis = 159, .raxis = 1680}, - {.oaxis = 168, .raxis = 285 }, - {.oaxis = 195, .raxis = 2630}, - {.oaxis = 211, .raxis = 1204}, - {.oaxis = 225, .raxis = 3429}, - {.oaxis = 249, .raxis = 2022}, - {.oaxis = 272, .raxis = 629 }, - {.oaxis = 265, .raxis = 2934}, - {.oaxis = 277, .raxis = 1499}, - {.oaxis = 289, .raxis = 124 }, - {.oaxis = 300, .raxis = 2349}, - {.oaxis = 311, .raxis = 959 }, - {.oaxis = 321, .raxis = 3199}, - {.oaxis = 331, .raxis = 1824}, - {.oaxis = 341, .raxis = 389 }, - {.oaxis = 351, .raxis = 2674}, - {.oaxis = 360, .raxis = 1268}, - {.oaxis = 364, .raxis = 3483}, - {.oaxis = 378, .raxis = 2118}, - {.oaxis = 387, .raxis = 733 }, - {.oaxis = 416, .raxis = 3028}, - {.oaxis = 404, .raxis = 1623}, - {.oaxis = 433, .raxis = 248 }, - {.oaxis = 421, .raxis = 2473}, - {.oaxis = 429, .raxis = 1098}, - {.oaxis = 437, .raxis = 3303}, - {.oaxis = 445, .raxis = 1948}, - {.oaxis = 443, .raxis = 543 }, - {.oaxis = 450, .raxis = 2828}, - {.oaxis = 448, .raxis = 1422}, - {.oaxis = 455, .raxis = 47 }, - {.oaxis = 453, .raxis = 2272}, - {.oaxis = 450, .raxis = 897 } -}; -PHY_ANT_ANGLE antNrrwPattern_1[L1_ANT_PATTERN_NUM] = -{ - {.oaxis = 0 , .raxis = 0 }, - {.oaxis = 139, .raxis = 1155}, - {.oaxis = 152, .raxis = 3380}, - {.oaxis = 138, .raxis = 2085}, - {.oaxis = 159, .raxis = 610 }, - {.oaxis = 168, .raxis = 2815}, - {.oaxis = 195, .raxis = 1560}, - {.oaxis = 211, .raxis = 134 }, - {.oaxis = 225, .raxis = 2359}, - {.oaxis = 249, .raxis = 952 }, - {.oaxis = 272, .raxis = 3159}, - {.oaxis = 265, .raxis = 1864}, - {.oaxis = 277, .raxis = 429 }, - {.oaxis = 289, .raxis = 2654}, - {.oaxis = 300, .raxis = 1279}, - {.oaxis = 311, .raxis = 3489}, - {.oaxis = 321, .raxis = 2129}, - {.oaxis = 331, .raxis = 754 }, - {.oaxis = 341, .raxis = 2919}, - {.oaxis = 351, .raxis = 1604}, - {.oaxis = 360, .raxis = 198 }, - {.oaxis = 364, .raxis = 2413}, - {.oaxis = 378, .raxis = 1048}, - {.oaxis = 387, .raxis = 3263}, - {.oaxis = 416, .raxis = 1958}, - {.oaxis = 404, .raxis = 553 }, - {.oaxis = 433, .raxis = 2778}, - {.oaxis = 421, .raxis = 1403}, - {.oaxis = 429, .raxis = 28 }, - {.oaxis = 437, .raxis = 2233}, - {.oaxis = 445, .raxis = 878 }, - {.oaxis = 443, .raxis = 3073}, - {.oaxis = 450, .raxis = 1758}, - {.oaxis = 448, .raxis = 352 }, - {.oaxis = 455, .raxis = 2577}, - {.oaxis = 453, .raxis = 1202}, - {.oaxis = 450, .raxis = 3427}, -}; /************************** Function Prototypes ******************************/ -static void LockStateUpdate(L2_SCAN_FSM *scanFsmPtr, bool state); static void AdvRecvStateUpdate(L2_SCAN_FSM *scanFsmPtr, bool state); -static void StageUpdate(L2_SCAN_FSM *scanFsmPtr, SCAN_STAGE newStage); -static void PendCfmAck(L2_SCAN_FSM *scanFsmPtr); -static int32_t Scan_InitScanHandler(L2_INST *instPtr); static int32_t Scan_AdvHandler(L2_INST *instPtr); -static int32_t Scan_CfmHandler(L2_INST *instPtr); static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId); -/** - * @brief Update Init-Scan lock state. - */ -static inline void LockStateUpdate(L2_SCAN_FSM *scanFsmPtr, bool state) -{ - ASSERT(scanFsmPtr != NULL); - DBG_INFO("Update BeamLockState from [%" PRId8 "] to [%" PRId8 "].\n", - scanFsmPtr->isLocked, state); - scanFsmPtr->isLocked = state; - return; -} - /** * @brief Update Base-info Advertisement state. */ @@ -159,91 +57,13 @@ static void AdvRecvStateUpdate(L2_SCAN_FSM *scanFsmPtr, bool state) return; } -/** - * @brief Update 2-Way Acquisition ack state. - */ -static void PendCfmAck(L2_SCAN_FSM *scanFsmPtr) -{ - ASSERT(scanFsmPtr != NULL); - DBG_INFO("Pending Cfm-Ack...\n"); - scanFsmPtr->isAckPending = true; - return; -} - /*****************************************************************************/ /** - * @brief Update current Scan-Net stage + * @brief Member node scan net BmAdvt stage handler. * - * @param scanFsmPtr A pointer to scan net fsm. - * newStage New state. + * @param scanFsmPtr A pointer to scan net fsm. * - * @return NONE. - * - ******************************************************************************/ -static void StageUpdate(L2_SCAN_FSM *scanFsmPtr, SCAN_STAGE newStage) -{ - ASSERT((newStage >= L2_SCAN_NONE_STAGE) && (newStage <= L2_SCAN_CFM_STAGE)); - DBG_INFO("ScanNet current stage: %" PRIu8 " -> ScanNet new stage: %" PRIu8 " \n", - scanFsmPtr->scanStage, newStage); - scanFsmPtr->scanStage = newStage; -} - -/*****************************************************************************/ -/** - * @brief Member node scan net InitScan stage handler. - * - * @param instPtr A pointer to l2 instance - * - * @return used slot (5ms slot). - * - ******************************************************************************/ -static int32_t Scan_InitScanHandler(L2_INST *instPtr) -{ - ASSERT(instPtr != NULL); - PHY_INST *phyRef = instPtr->config.phyInstPtr; - L2_SCAN_FSM *scanFsmPtr = instPtr->scanFSM; - DBG_INFO("** Net-Setup - InitScan Procedure (Attempts: %" PRIu8 ")**\n", scanFsmPtr->scanCnt); - - if (scanFsmPtr->scanCnt == 0) - { - // Update scan beam table - L2_Scan_RefreashScanTable(instPtr); - } - else if (Phy_IsFrameDetected(phyRef)) - { - // last scan beam number - scanFsmPtr->beamNo = Phy_LastScanBeamNo(phyRef); - // Update scan beam table - L2_Scan_RefreashScanTable(instPtr); - } - - if (scanFsmPtr->isLocked == true) - { - scanFsmPtr->beamNo = Phy_LastScanBeamNo(phyRef); - Phy_SetBeamNo(phyRef, scanFsmPtr->beamNo, PHY_ANT_BEAM_16); - L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_ADV_FRAME); - StageUpdate(scanFsmPtr, L2_SCAN_ADV_STAGE); - instPtr->log.lastBeamWidth = PHY_ANT_BEAM_16; - } /* Otherwise remain L2_SCAN_INIT_STAGE */ - else - { - /* Antenna steering (reserved, automatically accomplished by physical layer) */ - Phy_SetBeamNo(phyRef, scanFsmPtr->beamNo, PHY_ANT_BEAM_16); - L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_SCAN_FRAME); - scanFsmPtr->scanCnt++; - instPtr->log.lastBeamWidth = PHY_ANT_BEAM_16; - } - return SUCCESS; -} - -/*****************************************************************************/ -/** - * @brief Member node scan net BmAdvt stage handler. - * - * @param scanFsmPtr A pointer to scan net fsm. - * mainState A pointer to main node state. - * - * @return used slot (5ms slot). + * @return used slot (2 ms slot). * ******************************************************************************/ static int32_t Scan_AdvHandler(L2_INST *instPtr) @@ -252,92 +72,29 @@ static int32_t Scan_AdvHandler(L2_INST *instPtr) L2_SCAN_FSM *scanFsmPtr = instPtr->scanFSM; PHY_INST *phyRef = instPtr->config.phyInstPtr; TMC_INST *tmcRef = instPtr->config.tmcInstPtr; - DBG_INFO("** Net-Setup - Advertisement Procedure (Timeout: %" PRIu8 ") **\n", scanFsmPtr->advTimeout); + DBG_INFO("** Net-Setup - Advertisement Procedure (Attempts: %" PRIu8 ") **\n", scanFsmPtr->advCnt); // if received a valid bm frame. if (scanFsmPtr->isAdvRecvd == true) { - // last scan beam number - scanFsmPtr->beamNo = Phy_LastScanBeamNo(phyRef); - // reset listen counter. - scanFsmPtr->advTimeout = 0; + // antenna index from listening state + scanFsmPtr->antIndex = Phy_ListenAntIndex(phyRef); - // Update scan stage - StageUpdate(scanFsmPtr, L2_SCAN_CFM_STAGE); - - // start a confirm frame reception if in the target slot - if ((TimeCtrl_GetIntCnt(tmcRef) + 1) % CFM_FRAME_SLOTS == 0) + // start a acknowledge frame reception if in the target slot + if ((TimeCtrl_GetIntCnt(tmcRef) + 1) % ACK_FRAME_SLOTS == 0) // FIXME: correct? { - // using pull mode to receive confirm frame + // using pull mode to transmit confirm frame // since first time sync procedure is accomplished in ADV stage. - (void)Phy_SetBeamWidth(phyRef, PHY_ANT_BEAM_16); - (void)Phy_SetBeamNo(phyRef, scanFsmPtr->beamNo, PHY_ANT_BEAM_16); + (void)Phy_SetAntIndex(phyRef, scanFsmPtr->antIndex); (void)Phy_SetMode(phyRef, PHY_MODE_POLL); - (void)L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_CFM_FRAME); - scanFsmPtr->cfmTimeout++; // increase counter. - instPtr->log.lastBeamWidth = PHY_ANT_BEAM_16; + (void)L2_SetupNextSlot(instPtr, L2_TOR_TX, L2_FRAME_ACK); } - // otherwise skip... - return SUCCESS; } // no valid bm frame was received. else { - /* if time out reached, go back to init scan stage. */ - if (scanFsmPtr->advTimeout > ADV_LISTEN_TIMEOUT) - { - /* Antenna steering (reserved, automatically accomplished by physical layer) */ - (void)Phy_SetBeamNo(phyRef, scanFsmPtr->beamNo, PHY_ANT_BEAM_16); - (void)L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_SCAN_FRAME); - DBG_WARNING("Adv timeout, restart net scan from L2_SCAN_INIT_STAGE.\n"); - (void)L2_Scan_Reset(scanFsmPtr); - instPtr->log.lastBeamWidth = PHY_ANT_BEAM_16; - } - else - { - /* Antenna steering (reserved, automatically accomplished by physical layer) */ - (void)Phy_SetBeamNo(phyRef, scanFsmPtr->beamNo, PHY_ANT_BEAM_16); - (void)L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_ADV_FRAME); - scanFsmPtr->advTimeout++; - instPtr->log.lastBeamWidth = PHY_ANT_BEAM_16; - } - return SUCCESS; - } -} - -/*****************************************************************************/ -/** - * @brief Scan net 2Way-Acq stage handler. - * - * @param instPtr a pointer to linklayer layer instance - * - * @return used slot (5ms slot). - * - ******************************************************************************/ -static int32_t Scan_CfmHandler(L2_INST *instPtr) -{ - ASSERT(instPtr != NULL); - L2_SCAN_FSM *scanFsmPtr = instPtr->scanFSM; - PHY_INST *phyRef = instPtr->config.phyInstPtr; - DBG_INFO("** Net-Setup - Confirm Procedure (Timeout: %" PRIu8 ") **\n", scanFsmPtr->cfmTimeout); - - // 2 way acq failed, back to init stage. - if (scanFsmPtr->cfmTimeout >= CFM_LISTEN_TIMEOUT) - { - DBG_WARNING("Cmf timeout, restart net scan from L2_SCAN_INIT_STAGE.\n"); - L2_Scan_Reset(scanFsmPtr); - return SUCCESS; - } - if (L2_GetNextSlotNum(instPtr) % CFM_FRAME_SLOTS == 0) - { - // using pull mode to receive confirm frame - // since first time sync procedure is accomplished in ADV stage. - (void)Phy_SetBeamWidth(phyRef, PHY_ANT_BEAM_16); - (void)Phy_SetBeamNo(phyRef, scanFsmPtr->beamNo, PHY_ANT_BEAM_16); - (void)Phy_SetMode(phyRef, PHY_MODE_POLL); - (void)L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_CFM_FRAME); - scanFsmPtr->cfmTimeout++; // increase counter. - instPtr->log.lastBeamWidth = PHY_ANT_BEAM_16; + (void)L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_FRAME_ADV); + scanFsmPtr->advCnt++; } return SUCCESS; } @@ -376,9 +133,9 @@ static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dst L2_FrameInsertChunk(buff, L2_MSG_DST_ID, L2_MSG_DST_ID_LEN, &dataChunk); switch (type) { - case L2_ADV_FRAME: + case L2_FRAME_ADV: { - dataChunk = L2_ADV_FRAME; + dataChunk = L2_FRAME_ADV; L2_FrameInsertChunk(buff, L2_MSG_TYPE, L2_MSG_TYPE_LEN, &dataChunk); SysMonitor_GetSysinfo(&(instPtr->sysInfo)); @@ -399,9 +156,9 @@ static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dst } break; } - case L2_CFM_FRAME: + case L2_FRAME_ACK: { - dataChunk = L2_CFM_FRAME; + dataChunk = L2_FRAME_ACK; L2_FrameInsertChunk(buff, L2_MSG_TYPE, L2_MSG_TYPE_LEN, &dataChunk); SysMonitor_GetSysinfo(&(instPtr->sysInfo)); @@ -468,31 +225,29 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) ASSERT(msg != NULL); int32_t Status; L2_SCAN_FSM *scanFsmPtr = instPtr->scanFSM; - L2_FRAME_TYPE frameType = L2_NONE_FRAME; + L2_FRAME_TYPE frameType = L2_FRAME_NONE; L2_FrameGetChunk(msg, L2_MSG_TYPE, L2_MSG_TYPE_LEN, &frameType); + switch (frameType) { - case L2_SCAN_FRAME: - case L2_ADV_FRAME: + case L2_FRAME_ADV: { - if (L2_GetLastOptFrameType(instPtr) == L2_SCAN_FRAME) - { - LockStateUpdate(scanFsmPtr, true); - StageUpdate(scanFsmPtr, L2_SCAN_ADV_STAGE); - } if (L2_GetNodeType(instPtr) == L2_NODE_HEAD) { - DBG_ERROR("Unexpected call from head node, only member Node can handle L2_SCAN_FRAME and L2_ADV_FRAME.\n"); + DBG_ERROR("Unexpected call from head node, only member Node can handle L2_FRAME_ADV.\n"); Status = EUNEXP; break; } - uint8_t nodeId; - L2_FrameGetChunk(msg, L2_MSG_SRC_ID, L2_MSG_SRC_ID_LEN, &nodeId); + + L2_MSG_ADV *message = msg; + + // Update node status. + uint8_t nodeId = message->common.src; NODE_ENTRY *node; node = L2_GetNodeById(instPtr->nodeList, nodeId); if (node == NULL) { - node = L2_AppendNode(instPtr->nodeList, nodeId, L2_NODE_MEMBER); + node = L2_AppendNode(instPtr->nodeList, nodeId, L2_NODE_HEAD); if (node == NULL) { DBG_ERROR("Failed to append new node [id: %" PRId8 "]", nodeId); @@ -501,30 +256,26 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) } } // Update node info. - float nodePos; - L2_FrameGetChunk(msg, L2_MSG_ADV_SRC_POSX, L2_MSG_ADV_SRC_POSX_LEN, &nodePos); - node->nodeInfo.pos.x = (double)nodePos; - L2_FrameGetChunk(msg, L2_MSG_ADV_SRC_POSY, L2_MSG_ADV_SRC_POSY_LEN, &nodePos); - node->nodeInfo.pos.y = (double)nodePos; - L2_FrameGetChunk(msg, L2_MSG_ADV_SRC_POSZ, L2_MSG_ADV_SRC_POSZ_LEN, &nodePos); - node->nodeInfo.pos.z = (double)nodePos; - node->nodeInfo.beamNo = Phy_LastScanBeamNo(instPtr->config.phyInstPtr); + 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); // Reset node ttl node->ttlCnt = 0; - uint32_t netstartTime; // Update net start time. - L2_FrameGetChunk(msg, L2_MSG_ADV_SRC_NETSTART_TIME, L2_MSG_ADV_SRC_NETSTART_TIME_LEN, &netstartTime); - L2_EVENT netSetupDoneEvent = { + uint32_t netstartTime = message->srcNetStatTime; + L2_EVENT netSetupDoneEvent = + { .event = L2_EV_NET_ADV_UPDATE, - .msg = &netstartTime}; + .msg = &netstartTime + }; AssertL2Event(instPtr, &netSetupDoneEvent); // Calculate time deviation - uint32_t frameNum, slotNum = 0; - L2_FrameGetChunk(msg, L2_MSG_FRAME_NUM, L2_MSG_FRAME_NUM_LEN, &frameNum); - L2_FrameGetChunk(msg, L2_MSG_SLOT_NUM, L2_MSG_SLOT_NUM_LEN, &slotNum); + uint32_t frameNum = message->common.frameNum; + uint32_t slotNum = (uint32_t)message->common.slotNum; Status = L2_GetTimeDeviation(instPtr, frameNum, slotNum, true, &(instPtr->log.timeDev)); if (Status != SUCCESS) { @@ -532,18 +283,56 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) break; } + // check if net setup finished. + if (L2_isCxn2Net(instPtr) == false) + { + // Assert Net-Setup Done. + L2_EVENT l2Event = { + .event = L2_EV_NET_SETUP_DONE, + .msg = NULL + }; + (void)AssertL2Event(instPtr, &l2Event); + + // Assert update routine event... + L2_RTNE_CONFIG newRtneConfig = {0}; + L2_EVENT rtneUpEvent = + { + .event = L2_ROUTINE_UPDATE, + .msg = &newRtneConfig + }; + newRtneConfig.rtneTable = initTable; + newRtneConfig.rtnePrd = ARRAY_SIZE(initTable); + (void)AssertL2Event(instPtr, &rtneUpEvent); + } // Time sync. L2_TimeAdjust(instPtr, &(instPtr->log.timeDev)); + // Get data payload + uint32_t dataId = message->advDataId; + 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))); + instPtr->log.rxLog.dataId = dataId; + // trigger flag AdvRecvStateUpdate(scanFsmPtr, true); Status = SUCCESS; break; } - case L2_CFM_FRAME: + case L2_FRAME_ACK: { - uint8_t nodeId; - L2_FrameGetChunk(msg, L2_MSG_SRC_ID, L2_MSG_SRC_ID_LEN, &nodeId); + if (L2_GetNodeType(instPtr) == L2_NODE_MEMBER) + { + DBG_ERROR("Unexpected call from member node, only head Node can handle L2_FRAME_ACK.\n"); + Status = EUNEXP; + break; + } + + L2_MSG_ACK *message = msg; + + // Update node status. + uint8_t nodeId = message->common.src; NODE_ENTRY *node; node = L2_GetNodeById(instPtr->nodeList, nodeId); if (node == NULL) @@ -557,28 +346,19 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) } } // Update node info. - float nodePos; - L2_FrameGetChunk(msg, L2_MSG_CFM_SRC_POSX, L2_MSG_CFM_SRC_POSX_LEN, &nodePos); - node->nodeInfo.pos.x = (double)nodePos; - L2_FrameGetChunk(msg, L2_MSG_CFM_SRC_POSY, L2_MSG_CFM_SRC_POSY_LEN, &nodePos); - node->nodeInfo.pos.y = (double)nodePos; - L2_FrameGetChunk(msg, L2_MSG_CFM_SRC_POSZ, L2_MSG_CFM_SRC_POSZ_LEN, &nodePos); - node->nodeInfo.pos.z = (double)nodePos; - float nodeSpeed; - L2_FrameGetChunk(msg, L2_MSG_CFM_SRC_SPEEDX, L2_MSG_CFM_SRC_SPEEDX_LEN, &nodeSpeed); - node->nodeInfo.speed.x = (double)nodePos; - L2_FrameGetChunk(msg, L2_MSG_CFM_SRC_SPEEDY, L2_MSG_CFM_SRC_SPEEDY_LEN, &nodeSpeed); - node->nodeInfo.speed.y = (double)nodePos; - L2_FrameGetChunk(msg, L2_MSG_CFM_SRC_SPEEDZ, L2_MSG_CFM_SRC_SPEEDZ_LEN, &nodeSpeed); - node->nodeInfo.speed.z = (double)nodePos; + node->nodeInfo.pos.x = message->srcPosX; + node->nodeInfo.pos.y = message->srcPosY; + node->nodeInfo.pos.z = message->srcPosZ; + node->nodeInfo.speed.x = message->srcSpeedX; + node->nodeInfo.speed.y = message->srcSpeedY; + node->nodeInfo.speed.z = message->srcSpeedZ; // Reset node ttl node->ttlCnt = 0; // Calculate time deviation - uint32_t frameNum, slotNum = 0; - L2_FrameGetChunk(msg, L2_MSG_FRAME_NUM, L2_MSG_FRAME_NUM_LEN, &frameNum); - L2_FrameGetChunk(msg, L2_MSG_SLOT_NUM, L2_MSG_SLOT_NUM_LEN, &slotNum); + uint32_t frameNum = message->common.frameNum; + uint32_t slotNum = (uint32_t)message->common.slotNum; Status = L2_GetTimeDeviation(instPtr, frameNum, slotNum, true, &(instPtr->log.timeDev)); if (Status != SUCCESS) { @@ -586,61 +366,22 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) break; } - if (L2_GetNodeType(instPtr) != L2_NODE_HEAD) + if (L2_GetMainState(instPtr) == L2_INIT_STATE) // FIXME: state switch should follow routing table { - // check if net setup finished. - if (L2_isCxn2Net(instPtr) == false) - { - // Assert Net-Setup Done. - L2_EVENT l2Event; - l2Event.event = L2_EV_NET_SETUP_DONE; - l2Event.msg = NULL; - AssertL2Event(instPtr, &l2Event); - - // Assert update routine event... - L2_RTNE_CONFIG newRtneConfig = {0}; - L2_EVENT rtneUpEvent = - { - .event = L2_ROUTINE_UPDATE, - .msg = &newRtneConfig - }; - newRtneConfig.rtneTable = initTable; - newRtneConfig.rtnePrd = ARRAY_SIZE(initTable); - (void)AssertL2Event(instPtr, &rtneUpEvent); - } - PendCfmAck(instPtr->scanFSM); - // Time sync. - L2_TimeAdjust(instPtr, &(instPtr->log.timeDev)); - } - else - { - if (L2_GetMainState(instPtr) == L2_INIT_STATE - && L2_GetNodeListLen(instPtr->nodeList) >= instPtr->config.netScale - 1) - { - // Assert Net-Setup Done. - L2_EVENT l2Event; - L2_NODE_STATE newState = L2_NORMAL_STATE; - l2Event.event = L2_EV_MAIN_STATE_UPDATE; - l2Event.msg = &newState; - (void)AssertL2Event(instPtr, &l2Event); - } + // Assert Net-Setup Done. + L2_EVENT l2Event; + L2_NODE_STATE newState = L2_NORMAL_STATE; + l2Event.event = L2_EV_MAIN_STATE_UPDATE; + l2Event.msg = &newState; + (void)AssertL2Event(instPtr, &l2Event); } // Get data payload - uint32_t dataId; - L2_FrameGetChunk(msg, L2_MSG_CFM_PAYLOAD_ID, L2_MSG_CFM_PAYLOAD_ID_LEN, &dataId); - - if (L2_GetNodeType(instPtr) == L2_NODE_HEAD) - { - SysMonitor_UpdateRecvPayload(msg + L2_MSG_CFM_PAYLOAD, dataId, PAYLOAD_LEN_FLJ_RX); - } - else - { - SysMonitor_UpdateRecvPayload(msg + L2_MSG_CFM_PAYLOAD, dataId, PAYLOAD_LEN_LJ_RX); - } + uint32_t dataId = message->advDataId; + SysMonitor_UpdateRecvPayload(message->advData, dataId, L2_PAYLOAD_LEN_ADV); // Logging... - memcpy(instPtr->log.rxLog.data, msg, L2_CMF_PACK_LEN); + memcpy(instPtr->log.rxLog.data, msg, Phy_GetFrameLen(L2_L2frame2L1frame(L2_FRAME_ACK, L2_SVC_NONE))); instPtr->log.rxLog.dataId = dataId; Status = SUCCESS; @@ -673,8 +414,7 @@ int32_t L2_Scan_Init(L2_SCAN_FSM **scanFSMPtr) return ENOMEM; } L2_Scan_Reset(*scanFSMPtr); - (*scanFSMPtr)->beamNo = 0; - (*scanFSMPtr)->tableNo = 0; + (*scanFSMPtr)->antIndex = 0; return SUCCESS; } @@ -693,10 +433,8 @@ void L2_Scan_Reset(L2_SCAN_FSM *scanFSMPtr) scanFSMPtr->isLocked = false; scanFSMPtr->isAdvRecvd = false; scanFSMPtr->isAckPending = false; - scanFSMPtr->scanCnt = 0; - scanFSMPtr->advTimeout = 0; + scanFSMPtr->advCnt = 0; scanFSMPtr->cfmTimeout = 0; - scanFSMPtr->scanStage = L2_SCAN_NONE_STAGE; } /*****************************************************************************/ @@ -713,39 +451,13 @@ void L2_Scan_Destroy(L2_SCAN_FSM *scanFSMPtr) free(scanFSMPtr); } -/*****************************************************************************/ -/** - * @brief Refresh scan beam table. - * - * @param instPtr a pointer to linklayer layer instance - * - * @return NONE - * - ******************************************************************************/ -void L2_Scan_RefreashScanTable(L2_INST *instPtr) -{ - ASSERT(instPtr != NULL); - L2_SCAN_FSM *scanFsmPtr = instPtr->scanFSM; - PHY_INST *phyRef = instPtr->config.phyInstPtr; - if (scanFsmPtr->tableNo == 0) - { - Phy_SetBeamTable(phyRef, antNrrwPattern_0, scanFsmPtr->beamNo); - scanFsmPtr->tableNo = 1; - } - else - { - Phy_SetBeamTable(phyRef, antNrrwPattern_1, scanFsmPtr->beamNo); - scanFsmPtr->tableNo = 0; - } -} - /*****************************************************************************/ /** * @brief Execute a member node scan net procedure. * * @param instPtr a pointer to linklayer layer instance * - * @return used slot (5ms slot). + * @return used slot (2 ms slot). * ******************************************************************************/ int32_t L2_Scan_Proc(L2_INST *instPtr) @@ -758,27 +470,9 @@ int32_t L2_Scan_Proc(L2_INST *instPtr) } int32_t Status; DBG_INFO("-------- ScanNet Process --------\n"); - switch (scanFSM->scanStage) - { - case L2_SCAN_NONE_STAGE: - /* Start Scan. */ - StageUpdate(scanFSM, L2_SCAN_INIT_STAGE); - Status = Scan_InitScanHandler(instPtr); - break; - case L2_SCAN_INIT_STAGE: - Status = Scan_InitScanHandler(instPtr); - break; - case L2_SCAN_ADV_STAGE: - Status = Scan_AdvHandler(instPtr); - break; - case L2_SCAN_CFM_STAGE: - Status = Scan_CfmHandler(instPtr); - break; - default: - DBG_ERROR("Unexpected scan net stage: %" PRId8 ".\n", scanFSM->scanStage); - Status = FAILURE; - break; - } + + Status = Scan_AdvHandler(instPtr); + return Status; } @@ -789,7 +483,7 @@ int32_t L2_Scan_Proc(L2_INST *instPtr) * * @param rtneEntry Routine configuration * - * @return used slot (5ms slot). + * @return used slot (2 ms slot). * ******************************************************************************/ int32_t L2_Scan_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntry) @@ -797,106 +491,68 @@ 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) { - return SUCCESS; // skip - } - - if (L2_GetNodeType(instPtr) != L2_NODE_HEAD) - { - if (rtneEntry->dst == L2_GetNodeId(instPtr) || rtneEntry->dst == L2_BROADCAST_ID) - { - torState = L2_TOR_RX; - commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->src); - } - else if (rtneEntry->src == L2_GetNodeId(instPtr) || rtneEntry->src == L2_BROADCAST_ID) - { - torState = L2_TOR_TX; - commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->dst); - } - else - { - torState = L2_TOR_SUSPEND; - } - - // Setup antenna beam - Phy_SetBeamWidth(phyRef, PHY_ANT_BEAM_16); - instPtr->log.lastBeamWidth = PHY_ANT_BEAM_16; - if (commNode != NULL) - { - (void)Phy_SetBeamNo(phyRef, commNode->nodeInfo.beamNo, PHY_ANT_BEAM_16); - } - } - else - { - if (rtneEntry->dst == L2_GetNodeId(instPtr)) - { - torState = L2_TOR_RX; - } - else if (rtneEntry->src == L2_GetNodeId(instPtr)) - { - torState = L2_TOR_TX; - } - else - { - torState = L2_TOR_SUSPEND; - } - // Setup antenna beam - (void)Phy_SetBeamWidth(phyRef, PHY_ANT_BEAM_120); - instPtr->log.lastBeamWidth = PHY_ANT_BEAM_120; + return SUCCESS; // skip other slots, only process in the 1st slot } switch (rtneEntry->slotType) { - case L2_SLOT_SCAN: - if (L2_GetNodeType(instPtr) != L2_NODE_HEAD && torState == L2_TOR_TX) + case L2_SLOT_ADVERTISE: + if (L2_GetNodeType(instPtr) == L2_NODE_HEAD && rtneEntry->src == L2_GetNodeId(instPtr)) { - DBG_ERROR("Only head node can send init-scan Frame.\n"); + 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 + + memset(instPtr->sendPktData, 0xAA, L2_PACK_LEN_MAX); + if (ScanMsg_Packing(instPtr, L2_FRAME_ADV, rtneEntry->dst) != SUCCESS) + { + return FAILURE; + } + } + 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 + } + else + { + DBG_ERROR("Unsupported node type" PRId8 "or wrong routine table in L2_SLOT_ADVERTISE.\n", L2_GetNodeType(instPtr)); return FAILURE; } - (void)L2_SetupNextSlot(instPtr, torState, L2_SCAN_FRAME); + (void)Phy_SetAntIndex(instPtr->config.phyInstPtr, antIndex); + (void)L2_SetupNextSlot(instPtr, torState, L2_FRAME_ADV); break; - - case L2_SLOT_ADVERTISE: - if (torState == L2_TOR_TX) + case L2_SLOT_ACKNOWLEDGE: + if (L2_GetNodeType(instPtr) == L2_NODE_HEAD && rtneEntry->dst == L2_GetNodeId(instPtr)) { - if (L2_GetNodeType(instPtr) != L2_NODE_HEAD) - { - DBG_ERROR("Only head node can send ADV Frame.\n"); - return FAILURE; - } + antIndex = PHY_ANT_INDEX_NONE; // FIXME: is antenna index necessary here? + torState = L2_TOR_RX; // head node receives messages in acknowledge slot + } + 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_TX; // member node transmits message in acknowledge slot + memset(instPtr->sendPktData, 0xAA, L2_PACK_LEN_MAX); - if (ScanMsg_Packing(instPtr, L2_ADV_FRAME, rtneEntry->dst) != SUCCESS) + if (ScanMsg_Packing(instPtr, L2_FRAME_ACK, rtneEntry->dst) != SUCCESS) { return FAILURE; } } - /* Antenna setting (reserved.) */ - (void)L2_SetupNextSlot(instPtr, torState, L2_ADV_FRAME); - break; - - case L2_SLOT_CONFIRM: - if (torState == L2_TOR_TX) + else { - if (L2_GetNodeType(instPtr) != L2_NODE_HEAD) - { - if (instPtr->scanFSM->isAckPending == false) - { - DBG_INFO("Member node has no pending CFM-Ack frame.\n"); - break; - } - } - memset(instPtr->sendPktData, 0xAA, L2_PACK_LEN_MAX); - if (ScanMsg_Packing(instPtr, L2_CFM_FRAME, rtneEntry->dst) != SUCCESS) - { - return FAILURE; - } + DBG_ERROR("Unsupported node type" PRId8 "or wrong routine table in L2_SLOT_ACKNOWLEDGE.\n", L2_GetNodeType(instPtr)); + return FAILURE; } - /* Antenna setting (reserved.) */ - (void)L2_SetupNextSlot(instPtr, torState, L2_CFM_FRAME); + (void)Phy_SetAntIndex(instPtr->config.phyInstPtr, antIndex); + (void)L2_SetupNextSlot(instPtr, torState, L2_FRAME_ACK); break; default: DBG_ERROR("Cannot start a routine for slot type [%" PRId8 "] properly.\n", rtneEntry->slotType); diff --git a/df_datalink_sp/src/system_monitor.h b/df_datalink_sp/src/system_monitor.h index 62be2fa..19c4188 100644 --- a/df_datalink_sp/src/system_monitor.h +++ b/df_datalink_sp/src/system_monitor.h @@ -184,7 +184,7 @@ typedef struct __attribute__((packed)) double fsyncPAPR; ///< last frame sync peak-average ration uint8_t fsyncFreqCh; ///< last frame sync frequency estimation uint16_t deintlvAmpEst; ///< de-interleave amplitude estimation - uint8_t rfMode; ///< current rf working mode + uint8_t antControl; ///< current antenna rf mode uint8_t reserve[3]; ///< reserved data } PHYLOG;