From b9d7950b3a2fd81065ee0a55918676b11c8796a9 Mon Sep 17 00:00:00 2001 From: Gatsby Date: Wed, 14 May 2025 11:20:37 +0800 Subject: [PATCH] feature: code for vibration test --- df_datalink_ge/src/config/app_config.h | 2 +- df_datalink_sp/src/config/app_config.h | 2 +- .../src/networking/link_layer/link_layer.c | 8 ++- .../src/networking/link_layer/link_layer.h | 1 + .../src/networking/link_layer/msg_def.h | 12 +---- .../src/networking/link_layer/scan.c | 49 ------------------- .../src/networking/link_layer/service.c | 38 +++++++------- 7 files changed, 28 insertions(+), 84 deletions(-) diff --git a/df_datalink_ge/src/config/app_config.h b/df_datalink_ge/src/config/app_config.h index c7b80d6..fef8049 100644 --- a/df_datalink_ge/src/config/app_config.h +++ b/df_datalink_ge/src/config/app_config.h @@ -39,7 +39,7 @@ #define APP_EMULATE_DATA #define DATALINK_ENABLE_AUTORECONNECT #define DATALINK_ENABLE_AUTOTIMESYNC -//#define DATALINK_ENABLE_BERTEST +#define DATALINK_ENABLE_BERTEST 10 /************ Hardware Configs ************/ diff --git a/df_datalink_sp/src/config/app_config.h b/df_datalink_sp/src/config/app_config.h index 6768624..0773d2b 100644 --- a/df_datalink_sp/src/config/app_config.h +++ b/df_datalink_sp/src/config/app_config.h @@ -38,7 +38,7 @@ #define DATALINK_ENABLE_AUTORECONNECT #define DATALINK_ENABLE_AUTOTIMESYNC -// #define DATALINK_ENABLE_BERTEST +#define DATALINK_ENABLE_BERTEST 10 /************ Hardware Configs ************/ #define CPU_CLK_HZ XPAR_CPU_CORTEXA9_0_CPU_CLK_FREQ_HZ 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 f4aba4b..b78f235 100644 --- a/df_datalink_sp/src/networking/link_layer/link_layer.c +++ b/df_datalink_sp/src/networking/link_layer/link_layer.c @@ -163,7 +163,7 @@ static void L2_FrameStateUpdate(L2_INST *instPtr) #ifndef DATALINK_ENABLE_BERTEST isValid = L2_CrcCheck(instPtr->recvPktData, L2_SVC_PACK_LEN, L2_CRC16_1021); #else - isValid = true; + isValid = L2_Service_PktCheck(instPtr, instPtr->recvPktData); #endif instPtr->log.svcRecvCnt++; instPtr->log.svcCorrectCnt = isValid ? instPtr->log.svcCorrectCnt + 1 : instPtr->log.svcCorrectCnt; @@ -420,10 +420,14 @@ L1_FRAME_TYPE L2_L2frame2L1frame(L2_FRAME_TYPE l2Frame) case L2_SCAN_FRAME: return L1_10KBPS_10MS_FRAME; case L2_ADV_FRAME: - return L1_10KBPS_20MS_FRAME; case L2_CFM_FRAME: + return L1_10KBPS_20MS_FRAME; case L2_SVC_FRAME: +#if DATALINK_ENABLE_BERTEST == 10 + return L1_10KBPS_20MS_FRAME; +#else return L1_40KBPS_20MS_FRAME; +#endif default: return L1_NONE_FRAME; } 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 cbe8ad2..18d3476 100644 --- a/df_datalink_sp/src/networking/link_layer/link_layer.h +++ b/df_datalink_sp/src/networking/link_layer/link_layer.h @@ -173,6 +173,7 @@ int32_t L2_Scan_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *schedEntry) int32_t L2_Service_Init(L2_SVC_FSM **svcFSMPtr); void L2_Service_Reset(L2_SVC_FSM *svcFSMPtr); void L2_Service_Destroy(L2_SVC_FSM *svcFSMPtr); +bool L2_Service_PktCheck(L2_INST *instPtr, uint8_t *msg); int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg); int32_t L2_Service_Proc(L2_INST *instPtr); int32_t L2_Service_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *schedEntry); 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 6c03db3..da187a0 100644 --- a/df_datalink_sp/src/networking/link_layer/msg_def.h +++ b/df_datalink_sp/src/networking/link_layer/msg_def.h @@ -81,17 +81,7 @@ #define L2_MSG_CFM_SRC_POSY_LEN 4 #define L2_MSG_CFM_SRC_POSZ 16 #define L2_MSG_CFM_SRC_POSZ_LEN 4 -#define L2_MSG_CFM_SRC_SPEEDX 20 -#define L2_MSG_CFM_SRC_SPEEDX_LEN 4 -#define L2_MSG_CFM_SRC_SPEEDY 24 -#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 26 #define L2_MSG_CFM_CRC_LEN 2 // service payload structure: diff --git a/df_datalink_sp/src/networking/link_layer/scan.c b/df_datalink_sp/src/networking/link_layer/scan.c index d66824a..9fe17ad 100644 --- a/df_datalink_sp/src/networking/link_layer/scan.c +++ b/df_datalink_sp/src/networking/link_layer/scan.c @@ -409,27 +409,6 @@ static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dst L2_FrameInsertChunk(buff, L2_MSG_CFM_SRC_POSX, L2_MSG_CFM_SRC_POSX_LEN, &(instPtr->sysInfo.posX)); L2_FrameInsertChunk(buff, L2_MSG_CFM_SRC_POSY, L2_MSG_CFM_SRC_POSY_LEN, &(instPtr->sysInfo.posY)); L2_FrameInsertChunk(buff, L2_MSG_CFM_SRC_POSZ, L2_MSG_CFM_SRC_POSZ_LEN, &(instPtr->sysInfo.posZ)); - L2_FrameInsertChunk(buff, L2_MSG_CFM_SRC_SPEEDX, L2_MSG_CFM_SRC_SPEEDX_LEN, &(instPtr->sysInfo.vX)); - 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) @@ -437,10 +416,6 @@ static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dst DBG_ERROR("Fail to append CRC, error code: %" PRId32 ".\n", Status); return Status; } - - // Logging... - memcpy(instPtr->log.txLog.data, buff, L2_CMF_PACK_LEN); - instPtr->log.txLog.dataId = dataId; break; } default: @@ -564,13 +539,6 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) 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; // Reset node ttl node->ttlCnt = 0; @@ -626,23 +594,6 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) } } - // 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); - } - - // Logging... - memcpy(instPtr->log.rxLog.data, msg, L2_CMF_PACK_LEN); - instPtr->log.rxLog.dataId = dataId; - Status = SUCCESS; break; } diff --git a/df_datalink_sp/src/networking/link_layer/service.c b/df_datalink_sp/src/networking/link_layer/service.c index 2f83863..62a126e 100644 --- a/df_datalink_sp/src/networking/link_layer/service.c +++ b/df_datalink_sp/src/networking/link_layer/service.c @@ -155,8 +155,9 @@ static int32_t Service_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dst } return SUCCESS; #else - DBG_ERROR("Unexpected call of Service_Packing in BER test mode.\n"); - return EUNEXP; + size_t packLen = Phy_GetFrameLen(L2_L2frame2L1frame(L2_SVC_FRAME)); + memcpy(instPtr->sendPktData, testData, packLen); + return SUCCESS; #endif } @@ -260,20 +261,8 @@ int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg) } return Status; #else - uint32_t errorBits = 0; - for (int i = 0; i < L2_SVC_PACK_LEN; i++) - { - errorBits += count_ones(msg[i] ^ testData[i]); - } - - instPtr->serviceFSM->bitError += errorBits; - instPtr->serviceFSM->bitTotal += L2_SVC_PACK_LEN * 8; - // 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); - Status = L2_GetTimeDeviation(instPtr, frameNum, slotNum, true, &(instPtr->log.timeDev)); + Status = L2_GetTimeDeviation(instPtr, L2_GetLastFrameNum(instPtr), L2_GetLastSlotNum(instPtr), true, &(instPtr->log.timeDev)); if (Status != SUCCESS) { DBG_ERROR("Failed to compute time deviation.\n") @@ -289,6 +278,20 @@ int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg) #endif } +bool L2_Service_PktCheck(L2_INST *instPtr, uint8_t *msg) +{ + size_t packLen = Phy_GetFrameLen(L2_L2frame2L1frame(L2_SVC_FRAME)); + uint32_t errorBits = 0; + for (int i = 0; i < packLen; i++) + { + errorBits += count_ones(msg[i] ^ testData[i]); + } + + instPtr->serviceFSM->bitError += errorBits; + instPtr->serviceFSM->bitTotal += packLen * 8; + return (errorBits == 0); +} + /*****************************************************************************/ /** * @brief Initialize service net FSM @@ -406,12 +409,7 @@ int32_t L2_Service_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntr if (torState == L2_TOR_TX) { svcFSMPtr->txCnt++; -#ifndef DATALINK_ENABLE_BERTEST - // Packing service data Service_Packing(instPtr, L2_SVC_FRAME, rtneEntry->dst); -#else - memcpy(instPtr->sendPktData, testData, L2_SVC_PACK_LEN); -#endif } else {