feature: code for vibration test

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 ************/

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

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;
 	}

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);

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_CRC 26
-#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_LEN 2
 
 // service payload structure:

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;
 	}

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");
+	size_t packLen = Phy_GetFrameLen(L2_L2frame2L1frame(L2_SVC_FRAME));
-	return EUNEXP;
+	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;
+	Status = L2_GetTimeDeviation(instPtr, L2_GetLastFrameNum(instPtr), L2_GetLastSlotNum(instPtr), true, &(instPtr->log.timeDev));
-	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));
 	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
 		{