josh/hj_datalink
1
0
Fork 0
forked from gatsby/df_datalink
Code Issues Pull Requests Releases Activity

feat: 修改通过SDK编译

Browse Source
This commit is contained in:
josh
2025-05-22 22:14:10 +08:00
parent d8ed94bf2b
commit e2c4733a93
11 changed files with 535 additions and 258 deletions
Download Patch File Download Diff File Expand all files Collapse all files
df_datalink_sp/src/main.c
-1
View File
@@ -94,7 +94,6 @@ static PHY_CONFIG phyConfig = {
.devId = PHY_DEVICE_ID,
.baseAddr = PHY_BASEADDR,
.antBramId = PHY_ANTCONFIG_BRAM_DEVICE_ID,
.antInfoBramId = PHY_ANTINFO_BRAM_DEVICE_ID,
.txBramId = PHY_TXBUFF_BRAM_DEVICE_ID,
.rxBramId = PHY_RXBUFF_BRAM_DEVICE_ID,
.hwXadcId = PHY_HW_XADC_DEVICE_ID,
df_datalink_sp/src/networking/link_layer/framing.c
+1 -1
View File
@@ -81,7 +81,7 @@ void L2_FrameGetChunk(uint8_t *frame, size_t offset, size_t bytes, void *chunk)
* SUCCESS
*
******************************************************************************/
static int32_t L2_CrcGen(uint8_t *dataBuff, uint32_t dataLen, L2_CRC_TYPE crcType, uint32_t *crc)
int32_t L2_CrcGen(uint8_t *dataBuff, uint32_t dataLen, L2_CRC_TYPE crcType, uint32_t *crc)
{
ASSERT(dataBuff != NULL);
int32_t Status;
df_datalink_sp/src/networking/link_layer/link_layer.c
+1 -2
View File
@@ -1014,9 +1014,8 @@ static int32_t Linklayer_Routine(L2_INST *instPtr)
case L2_SLOT_DISTRIBUTE:
case L2_SLOT_MANAGE:
case L2_SLOT_RTT:
Status = L2_Service_Routine(instPtr, &rtneEntry, L2_SVC_128K);
case L2_SLOT_SERVICE:
Status = L2_Service_Routine(instPtr, &rtneEntry, L2_GetServiceRate(instPtr));
Status = L2_Service_Routine(instPtr, &rtneEntry);
break;
default:
DBG_ERROR("UNKNOWN slot type [%" PRId8 "].\n", rtneEntry.slotType);
df_datalink_sp/src/networking/link_layer/link_layer.h
+6 -9
View File
@@ -69,10 +69,10 @@ typedef struct
uint32_t ackRecvCnt; ///< acknowledgement received frame count
uint32_t ackCorrectCnt; ///< acknowledgement correctly received frame count
uint32_t distRxCnt; ///< distribution rx frame count
uint32_t distTxCnt; ///< distribution tx frame count
uint32_t distRecvCnt; ///< distribution received frame count
uint32_t distCorrectCnt;///< distribution correctly received frame count
uint32_t disRxCnt; ///< distribution rx frame count
uint32_t disTxCnt; ///< distribution tx frame count
uint32_t disRecvCnt; ///< distribution received frame count
uint32_t disCorrectCnt;///< distribution correctly received frame count
uint32_t mngRxCnt; ///< management rx frame count
uint32_t mngTxCnt; ///< management tx frame count
@@ -152,7 +152,7 @@ int32_t L2_SetupNextSlot(L2_INST *instPtr, L2_TOR_TYPE txrxMode, L2_FRAME_TYPE f
void L2_FrameInsertChunk(uint8_t *frame, size_t offset, size_t bytes, const void *chunk);
void L2_FrameGetChunk(uint8_t *frame, size_t offset, size_t bytes, void *chunk);
int32_t L2_CrcAppend(uint8_t *dataBuff, uint32_t dataLen, uint32_t crcPos, L2_CRC_TYPE crcType);
static int32_t L2_CrcGen(uint8_t *dataBuff, uint32_t dataLen, L2_CRC_TYPE crcType, uint32_t *crc);
int32_t L2_CrcGen(uint8_t *dataBuff, uint32_t dataLen, L2_CRC_TYPE crcType, uint32_t *crc);
bool L2_CrcCheck(uint8_t *dataBuff, uint32_t dataLen, L2_CRC_TYPE crcType);
/** @} LinkLayer_Framing*/
@@ -171,9 +171,6 @@ int32_t L2_RescheduleEventAfter(L2_INST *instPtr, L2_SCHD_EVENT *schdEvent, uint
* @defgroup LinkLayer_Scan
* @{
*/
void L2_Scan_RefreashScanTable(L2_INST *instPtr);
void L2_Scan_Reset(L2_SCAN_FSM *scanFSMPtr);
void L2_Scan_Destroy(L2_SCAN_FSM *scanFSMPtr);
int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg);
int32_t L2_Scan_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *schedEntry);
/** @} LinkLayer_Scan*/
@@ -231,4 +228,4 @@ PHY_ANT_INDEX L2_AntId2AntIndex(uint8_t antId);
#endif /* __LINK_LAYER_H_ */
/** @} LinkLayer*/
/** @} LinkLayer*/
df_datalink_sp/src/networking/link_layer/msg_def.h
+6 -6
View File
@@ -88,7 +88,7 @@ typedef struct
*/
typedef struct
{
L2_MSG_COMMON *common; ///< common message header
L2_MSG_COMMON common; ///< common message header
double srcPosX; ///< position x
double srcPosY; ///< position y
double srcPosZ; ///< position z
@@ -104,7 +104,7 @@ typedef struct
*/
typedef struct
{
L2_MSG_COMMON *common; ///< common message header
L2_MSG_COMMON common; ///< common message header
double srcPosX; ///< position x
double srcPosY; ///< position y
double srcPosZ; ///< position z
@@ -122,7 +122,7 @@ typedef struct
*/
typedef struct
{
L2_MSG_COMMON *common; ///< common message header
L2_MSG_COMMON common; ///< common message header
double srcPosX; ///< position x
double srcPosY; ///< position y
double srcPosZ; ///< position z
@@ -140,7 +140,7 @@ typedef struct
*/
typedef struct
{
L2_MSG_COMMON *common; ///< common message header
L2_MSG_COMMON common; ///< common message header
double srcPosX; ///< position x
double srcPosY; ///< position y
double srcPosZ; ///< position z
@@ -158,7 +158,7 @@ typedef struct
*/
typedef struct
{
L2_MSG_COMMON *common; ///< common message header
L2_MSG_COMMON common; ///< common message header
double srcPosX; ///< position x
double srcPosY; ///< position y
double srcPosZ; ///< position z
@@ -176,4 +176,4 @@ typedef struct
/** @} LinkLayer_MSG_Constants*/
/** @} LinkLayer*/
/** @} LinkLayer*/
df_datalink_sp/src/networking/link_layer/node_list.c
+3 -2
View File
@@ -140,7 +140,8 @@ void L2_NodeList_Destroy(NODE_LIST *list)
NODE_ENTRY *L2_GetNodeById(NODE_LIST *list, uint8_t nodeId)
{
ASSERT(list != NULL);
if (nodeId == L2_BROADCAST_ID || nodeId == L2_NONE_ID)
if (nodeId == L2_BROADCAST_ID1 || nodeId == L2_BROADCAST_ID2 ||
nodeId == L2_BROADCAST_ID3 || nodeId == L2_NONE_ID)
{
return NULL;
}
@@ -558,4 +559,4 @@ int32_t L2_IterDeleteNode(NODE_LIST *list, bool (*condition)(NODE_ENTRY *))
/** @} LinkLayer_NodeList*/
/** @} LinkLayer*/
/** @} LinkLayer*/
df_datalink_sp/src/networking/link_layer/scan.c
+20 -49
View File
@@ -60,14 +60,15 @@ static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dst
switch (type)
{
case L2_FRAME_ADV:
{
L2_MSG_ADV *message = (L2_MSG_ADV *)(instPtr->sendPktData);
// Common section of link layer package
message->common->frameNum = L2_GetNextFrameNum(instPtr);
message->common->slotNum = L2_GetNextSlotNum(instPtr);
message->common->src = L2_GetNodeId(instPtr);
message->common->dst = dstId;
message->common->msgType = L2_FRAME_ADV;
message->common.frameNum = L2_GetNextFrameNum(instPtr);
message->common.slotNum = L2_GetNextSlotNum(instPtr);
message->common.src = L2_GetNodeId(instPtr);
message->common.dst = dstId;
message->common.msgType = L2_FRAME_ADV;
// insert position and speed info
SysMonitor_GetSysinfo(&(instPtr->sysInfo));
@@ -76,10 +77,10 @@ static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dst
message->srcPosZ = instPtr->sysInfo.posZ;
// Insert net start time
message->srcNetStatTime = &(instPtr->netStartTime);
message->srcNetStatTime = instPtr->netStartTime;
// Packing extra payload
SysMonitor_GetSendPayload(message->advData, message->advDataId, L2_PAYLOAD_LEN_ADV);
SysMonitor_GetSendPayload(message->advData, &(message->advDataId), L2_PAYLOAD_LEN_ADV);
if (message->advDataId == DATA_ID_NONE)
{
memset(message->advData, 0xAA, L2_PAYLOAD_LEN_ADV);
@@ -87,7 +88,7 @@ static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dst
// Append CRC
uint32_t crcVal;
Status = L2_CrcGen(message, sizeof(L2_MSG_ADV) - L2_MSG_CRC_LEN, L2_CRC8_31, &crcVal);
Status = L2_CrcGen((uint8_t *)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);
@@ -100,14 +101,16 @@ static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dst
instPtr->log.txLog.dataId = message->advDataId;
break;
}
case L2_FRAME_ACK:
{
L2_MSG_ACK *message = (L2_MSG_ACK *)(instPtr->sendPktData);
// Common section of link layer package
message->common->frameNum = L2_GetNextFrameNum(instPtr);
message->common->slotNum = L2_GetNextSlotNum(instPtr);
message->common->src = L2_GetNodeId(instPtr);
message->common->dst = dstId;
message->common.frameNum = L2_GetNextFrameNum(instPtr);
message->common.slotNum = L2_GetNextSlotNum(instPtr);
message->common.src = L2_GetNodeId(instPtr);
message->common.dst = dstId;
message->common.msgType = L2_FRAME_ADV;
// insert position and speed info
@@ -120,7 +123,7 @@ static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dst
message->srcSpeedZ = instPtr->sysInfo.vZ;
// Packing extra payload
SysMonitor_GetSendPayload(message->ackData, message->ackDataId, L2_PAYLOAD_LEN_ACK);
SysMonitor_GetSendPayload(message->ackData, &(message->ackDataId), L2_PAYLOAD_LEN_ACK);
if (message->ackDataId == DATA_ID_NONE)
{
memset(message->ackData, 0xAA, L2_PAYLOAD_LEN_ACK);
@@ -128,7 +131,7 @@ static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dst
// Append CRC
uint32_t crcVal;
Status = L2_CrcGen(message, sizeof(L2_MSG_ACK) - L2_MSG_CRC_LEN, L2_CRC8_31, &crcVal);
Status = L2_CrcGen((uint8_t *)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);
@@ -141,6 +144,7 @@ static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dst
instPtr->log.txLog.dataId = message->ackDataId;
break;
}
default:
DBG_ERROR("Unexpected frame type: %" PRId8 ".\n", type);
return EPARAM;
@@ -305,8 +309,8 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg)
}
// Get data payload
uint32_t dataId = message->advDataId;
SysMonitor_UpdateRecvPayload(message->advData, dataId, L2_PAYLOAD_LEN_ADV);
uint32_t dataId = message->ackDataId;
SysMonitor_UpdateRecvPayload(message->ackData, dataId, L2_PAYLOAD_LEN_ACK);
// Logging...
memcpy(instPtr->log.rxLog.data, msg, sizeof(L2_MSG_ACK));
@@ -323,39 +327,6 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg)
return Status;
}
/*****************************************************************************/
/**
* @brief Reset ScanNet FSM
*
* @param scanFsmPtr a pointer to scan net finite state machine
*
* @return NONE
*
******************************************************************************/
void L2_Scan_Reset(L2_SCAN_FSM *scanFSMPtr)
{
ASSERT(scanFSMPtr != NULL);
scanFSMPtr->isLocked = false;
scanFSMPtr->isAdvRecvd = false;
scanFSMPtr->isAckPending = false;
scanFSMPtr->advCnt = 0;
scanFSMPtr->cfmTimeout = 0;
}
/*****************************************************************************/
/**
* @brief Destroy ScanNet FSM, free memory
*
* @param scanFsmPtr a pointer to scan net finite state machine
*
* @return NONE
*
******************************************************************************/
void L2_Scan_Destroy(L2_SCAN_FSM *scanFSMPtr)
{
free(scanFSMPtr);
}
/*****************************************************************************/
/**
df_datalink_sp/src/networking/link_layer/service.c
+458 -148
View File
@@ -36,6 +36,13 @@
/**************************** Type Definitions *******************************/
/************************** Function Prototypes ******************************/
static int32_t Service_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId);
static void Service_PackingCommon(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId);
static int32_t Service_Packing128K(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId);
static int32_t Service_Packing512K(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId);
static int32_t Service_Packing2M(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId);
static int32_t Service_PktHandler128K(L2_INST *instPtr, uint8_t *msg);
static int32_t Service_PktHandler512K(L2_INST *instPtr, uint8_t *msg);
static int32_t Service_PktHandler2M(L2_INST *instPtr, uint8_t *msg);
/************************** Variable Definitions *****************************/
static uint8_t testData[L2_PACK_LEN_MAX] =
@@ -97,116 +104,236 @@ static int32_t Service_Packing(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dst
case L2_FRAME_DIS:
case L2_FRAME_MNG:
case L2_FRAME_RTT:
L2_MSG_SVC_128K *message = (L2_MSG_SVC_128K *)(instPtr->sendPktData);
// Common section of link layer package
message->common->frameNum = L2_GetNextFrameNum(instPtr);
message->common->slotNum = L2_GetNextSlotNum(instPtr);
message->common->src = L2_GetNodeId(instPtr);
message->common->dst = dstId;
message->common.msgType = L2_FRAME_ADV;
// 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->sysInfo.vX;
message->srcSpeedY = instPtr->sysInfo.vY;
message->srcSpeedZ = instPtr->sysInfo.vZ;
// Packing extra payload
SysMonitor_GetSendPayload(message->svcData, message->svcDataId, L2_PAYLOAD_LEN_SVC_128K);
if (message->svcDataId == DATA_ID_NONE)
{
memset(message->svcData, 0xAA, L2_PAYLOAD_LEN_SVC_128K);
}
// Append CRC
uint32_t crcVal;
Status = L2_CrcGen(message, sizeof(L2_MSG_SVC_128K) - 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, sizeof(L2_MSG_SVC_128K));
instPtr->log.txLog.dataId = message->svcDataId;
{
Status = Service_Packing128K(instPtr, type, dstId);
break;
}
case L2_FRAME_SVC:
// retrieve basic info
size_t msgLen = L2_GetSvcPacketLen(instPtr->config.serviceRate);
size_t dataLen = L2_GetSvcPayloadLen(instPtr->config.serviceRate);
{
switch (instPtr->config.serviceRate)
{
case L2_SVC_128K:
L2_MSG_SVC_128K *message = (L2_MSG_SVC_128K *)(instPtr->sendPktData);
Status = Service_Packing128K(instPtr, type, dstId);
break;
case L2_SVC_512K:
L2_MSG_SVC_512K *message = (L2_MSG_SVC_512K *)(instPtr->sendPktData);
Status = Service_Packing512K(instPtr, type, dstId);
break;
case L2_SVC_2M:
L2_MSG_SVC_2M *message = (L2_MSG_SVC_2M *)(instPtr->sendPktData);
Status = Service_Packing2M(instPtr, type, dstId);
break;
default:
DBG_ERROR("Unsupported service rate: %" PRId8 ".\n", instPtr->config.serviceRate);
DBG_ERROR("Unexpected service rate: %" PRIu8 ".\n", instPtr->config.serviceRate);
return EPARAM;
}
// Common section of link layer package
message->common->frameNum = L2_GetNextFrameNum(instPtr);
message->common->slotNum = L2_GetNextSlotNum(instPtr);
message->common->src = L2_GetNodeId(instPtr);
message->common->dst = dstId;
message->common.msgType = L2_FRAME_ADV;
// 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->sysInfo.vX;
message->srcSpeedY = instPtr->sysInfo.vY;
message->srcSpeedZ = instPtr->sysInfo.vZ;
// Packing extra payload
SysMonitor_GetSendPayload(message->svcData, message->svcDataId, dataLen);
if (message->svcDataId == DATA_ID_NONE)
{
memset(message->svcData, 0xAA, dataLen);
}
// Append CRC
uint32_t crcVal;
Status = L2_CrcGen(message, msgLen - 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, msgLen);
instPtr->log.txLog.dataId = message->svcDataId;
break;
}
default:
DBG_ERROR("Unexpected frame type: %" PRIu8 ".\n", type);
return EPARAM;
break;
}
return SUCCESS;
return Status;
#else
DBG_ERROR("Unexpected call of Service_Packing in BER test mode.\n");
return EUNEXP;
#endif
}
/*****************************************************************************/
/**
* @brief Packing Service message (common part)
*
* @param instPtr pointer to linklayer instance
* type message type.
* dstId destination node address.
*
* @return EPARAM
* SUCCESS
*
******************************************************************************/
static void Service_PackingCommon(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId)
{
ASSERT(instPtr != NULL);
L2_MSG_COMMON *message = (L2_MSG_COMMON *)(instPtr->sendPktData);
// Common section of link layer package
message->frameNum = L2_GetNextFrameNum(instPtr);
message->slotNum = L2_GetNextSlotNum(instPtr);
message->src = L2_GetNodeId(instPtr);
message->dst = dstId;
message->msgType = type;
}
/*****************************************************************************/
/**
* @brief Packing Service message (128 kbps)
*
* @param instPtr pointer to linklayer instance
* type message type.
* dstId destination node address.
*
* @return EPARAM
* SUCCESS
*
******************************************************************************/
static int32_t Service_Packing128K(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId)
{
ASSERT(instPtr != NULL);
int32_t Status;
Service_PackingCommon(instPtr, type, dstId);
L2_MSG_SVC_128K *message = (L2_MSG_SVC_128K *)(instPtr->sendPktData);
size_t msgLen = sizeof(L2_MSG_SVC_128K);
size_t dataLen = L2_PAYLOAD_LEN_SVC_128K;
// 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->sysInfo.vX;
message->srcSpeedY = instPtr->sysInfo.vY;
message->srcSpeedZ = instPtr->sysInfo.vZ;
// Packing extra payload
SysMonitor_GetSendPayload(message->svcData, &(message->svcDataId), dataLen);
if (message->svcDataId == DATA_ID_NONE)
{
memset(message->svcData, 0xAA, dataLen);
}
// Append CRC
uint32_t crcVal;
Status = L2_CrcGen((uint8_t *)message, msgLen - 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, msgLen);
instPtr->log.txLog.dataId = message->svcDataId;
return SUCCESS;
}
/*****************************************************************************/
/**
* @brief Packing Service message (512 kbps)
*
* @param instPtr pointer to linklayer instance
* type message type.
* dstId destination node address.
*
* @return EPARAM
* SUCCESS
*
******************************************************************************/
static int32_t Service_Packing512K(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId)
{
ASSERT(instPtr != NULL);
int32_t Status;
Service_PackingCommon(instPtr, type, dstId);
L2_MSG_SVC_512K *message = (L2_MSG_SVC_512K *)(instPtr->sendPktData);
size_t msgLen = sizeof(L2_MSG_SVC_512K);
size_t dataLen = L2_PAYLOAD_LEN_SVC_512K;
// 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->sysInfo.vX;
message->srcSpeedY = instPtr->sysInfo.vY;
message->srcSpeedZ = instPtr->sysInfo.vZ;
// Packing extra payload
SysMonitor_GetSendPayload(message->svcData, &(message->svcDataId), dataLen);
if (message->svcDataId == DATA_ID_NONE)
{
memset(message->svcData, 0xAA, dataLen);
}
// Append CRC
uint32_t crcVal;
Status = L2_CrcGen((uint8_t *)message, msgLen - 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, msgLen);
instPtr->log.txLog.dataId = message->svcDataId;
return SUCCESS;
}
/*****************************************************************************/
/**
* @brief Packing Service message (2 Mbps)
*
* @param instPtr pointer to linklayer instance
* type message type.
* dstId destination node address.
*
* @return EPARAM
* SUCCESS
*
******************************************************************************/
static int32_t Service_Packing2M(L2_INST *instPtr, L2_FRAME_TYPE type, uint8_t dstId)
{
ASSERT(instPtr != NULL);
int32_t Status;
Service_PackingCommon(instPtr, type, dstId);
L2_MSG_SVC_2M *message = (L2_MSG_SVC_2M *)(instPtr->sendPktData);
size_t msgLen = sizeof(L2_MSG_SVC_2M);
size_t dataLen = L2_PAYLOAD_LEN_SVC_2M;
// 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->sysInfo.vX;
message->srcSpeedY = instPtr->sysInfo.vY;
message->srcSpeedZ = instPtr->sysInfo.vZ;
// Packing extra payload
SysMonitor_GetSendPayload(message->svcData, &(message->svcDataId), dataLen);
if (message->svcDataId == DATA_ID_NONE)
{
memset(message->svcData, 0xAA, dataLen);
}
// Append CRC
uint32_t crcVal;
Status = L2_CrcGen((uint8_t *)message, msgLen - 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, msgLen);
instPtr->log.txLog.dataId = message->svcDataId;
return SUCCESS;
}
/*****************************************************************************/
/**
* @brief Handle service received packet. Send data up to MPC
@@ -223,9 +350,8 @@ int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg)
ASSERT(msg != NULL);
int32_t Status;
#ifndef DATALINK_ENABLE_BERTEST
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);
size_t msgLen, dataLen;
// retrieve basic info
switch (frameType)
@@ -234,76 +360,35 @@ int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg)
if (L2_GetNodeType(instPtr) == L2_NODE_HEAD)
{
DBG_ERROR("Unexpected call from head node, only member Node can handle L2_FRAME_DIS.\n");
Status = EUNEXP;
break;
return EUNEXP;
}
case L2_FRAME_MNG:
case L2_FRAME_RTT:
L2_MSG_SVC_128K *message = (L2_MSG_SVC_128K *)msg;
msgLen = sizeof(L2_MSG_SVC_128K);
dataLen = L2_PAYLOAD_LEN_SVC_128K;
Status = Service_PktHandler128K(instPtr, msg);
break;
case L2_FRAME_SVC:
msgLen = L2_GetSvcPacketLen(instPtr->config.serviceRate);
dataLen = L2_GetSvcPayloadLen(instPtr->config.serviceRate);
switch (instPtr->config.serviceRate)
{
case L2_SVC_128K:
Status = Service_PktHandler128K(instPtr, msg);
break;
case L2_SVC_512K:
Status = Service_PktHandler512K(instPtr, msg);
break;
case L2_SVC_2M:
Status = Service_PktHandler2M(instPtr, msg);
break;
default:
DBG_ERROR("Unexpected service rate: %" PRIu8 ".\n", instPtr->config.serviceRate);
return EPARAM;
}
break;
default:
DBG_ERROR("Unsupported frame type: %" PRIu8 ".\n", frameType);
return EPARAM;
}
return Status;
// 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);
if (node == NULL)
{
DBG_ERROR("Failed to append new node [id: %" PRId8 "]", nodeId);
Status = ENOMEM;
break;
}
}
// Update node info.
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;
node->nodeInfo.antIndex = Phy_ReceiveAntIndex(instPtr->config.phyInstPtr);
// Reset node ttl
node->ttlCnt = 0;
// Calculate time deviation
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)
{
DBG_ERROR("Failed to compute time deviation.\n")
break;
}
// Get data payload
SysMonitor_UpdateRecvPayload(message->svcData, message->svcDataId, dataLen);
// Logging...
memcpy(instPtr->log.rxLog.data, msg, msgLen);
instPtr->log.rxLog.dataId = message->svcDataId;
#ifdef DATALINK_ENABLE_AUTOTIMESYNC
// Implement a auto time sync.
if (L2_GetNodeType(instPtr) != L2_NODE_HEAD)
{
// Time sync.
L2_TimeAdjust(instPtr, &(instPtr->log.timeDev));
}
#endif
#else
uint32_t errorBits = 0;
uint32_t pktLen = L2_GetSvcPacketLen(instPtr->config.serviceRate);
@@ -335,6 +420,231 @@ int32_t L2_Service_PktHandler(L2_INST *instPtr, uint8_t *msg)
#endif
}
/*****************************************************************************/
/**
* @brief Handle service received packet. Send data up to MPC. (128 kbps)
*
* @param msg A point32_ter the the message frame.
*
* @return SUCCESS message has been successfully handled.
* other error occurred.
*
******************************************************************************/
static int32_t Service_PktHandler128K(L2_INST *instPtr, uint8_t *msg)
{
ASSERT(instPtr != NULL);
ASSERT(msg != NULL);
int32_t Status;
L2_MSG_SVC_128K *message = (L2_MSG_SVC_128K *)msg;
size_t msgLen = sizeof(L2_MSG_SVC_128K);
size_t dataLen = L2_PAYLOAD_LEN_SVC_128K;
// 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);
if (node == NULL)
{
DBG_ERROR("Failed to append new node [id: %" PRId8 "]", nodeId);
return ENOMEM;
}
}
// Update node info.
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;
node->nodeInfo.antIndex = Phy_ReceiveAntIndex(instPtr->config.phyInstPtr);
// Reset node ttl
node->ttlCnt = 0;
// Calculate time deviation
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)
{
DBG_ERROR("Failed to compute time deviation.\n")
return Status;
}
// Get data payload
SysMonitor_UpdateRecvPayload(message->svcData, message->svcDataId, dataLen);
// Logging...
memcpy(instPtr->log.rxLog.data, msg, msgLen);
instPtr->log.rxLog.dataId = message->svcDataId;
#ifdef DATALINK_ENABLE_AUTOTIMESYNC
// Implement a auto time sync.
if (L2_GetNodeType(instPtr) != L2_NODE_HEAD)
{
// Time sync.
L2_TimeAdjust(instPtr, &(instPtr->log.timeDev));
}
#endif
return Status;
}
/*****************************************************************************/
/**
* @brief Handle service received packet. Send data up to MPC. (512 kbps)
*
* @param msg A point32_ter the the message frame.
*
* @return SUCCESS message has been successfully handled.
* other error occurred.
*
******************************************************************************/
static int32_t Service_PktHandler512K(L2_INST *instPtr, uint8_t *msg)
{
ASSERT(instPtr != NULL);
ASSERT(msg != NULL);
int32_t Status;
L2_MSG_SVC_512K *message = (L2_MSG_SVC_512K *)msg;
size_t msgLen = sizeof(L2_MSG_SVC_512K);
size_t dataLen = L2_PAYLOAD_LEN_SVC_512K;
// 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);
if (node == NULL)
{
DBG_ERROR("Failed to append new node [id: %" PRId8 "]", nodeId);
return ENOMEM;
}
}
// Update node info.
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;
node->nodeInfo.antIndex = Phy_ReceiveAntIndex(instPtr->config.phyInstPtr);
// Reset node ttl
node->ttlCnt = 0;
// Calculate time deviation
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)
{
DBG_ERROR("Failed to compute time deviation.\n")
return Status;
}
// Get data payload
SysMonitor_UpdateRecvPayload(message->svcData, message->svcDataId, dataLen);
// Logging...
memcpy(instPtr->log.rxLog.data, msg, msgLen);
instPtr->log.rxLog.dataId = message->svcDataId;
#ifdef DATALINK_ENABLE_AUTOTIMESYNC
// Implement a auto time sync.
if (L2_GetNodeType(instPtr) != L2_NODE_HEAD)
{
// Time sync.
L2_TimeAdjust(instPtr, &(instPtr->log.timeDev));
}
#endif
return Status;
}
/*****************************************************************************/
/**
* @brief Handle service received packet. Send data up to MPC. (2 Mbps)
*
* @param msg A point32_ter the the message frame.
*
* @return SUCCESS message has been successfully handled.
* other error occurred.
*
******************************************************************************/
static int32_t Service_PktHandler2M(L2_INST *instPtr, uint8_t *msg)
{
ASSERT(instPtr != NULL);
ASSERT(msg != NULL);
int32_t Status;
L2_MSG_SVC_2M *message = (L2_MSG_SVC_2M *)msg;
size_t msgLen = sizeof(L2_MSG_SVC_2M);
size_t dataLen = L2_PAYLOAD_LEN_SVC_2M;
// 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);
if (node == NULL)
{
DBG_ERROR("Failed to append new node [id: %" PRId8 "]", nodeId);
return ENOMEM;
}
}
// Update node info.
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;
node->nodeInfo.antIndex = Phy_ReceiveAntIndex(instPtr->config.phyInstPtr);
// Reset node ttl
node->ttlCnt = 0;
// Calculate time deviation
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)
{
DBG_ERROR("Failed to compute time deviation.\n")
return Status;
}
// Get data payload
SysMonitor_UpdateRecvPayload(message->svcData, message->svcDataId, dataLen);
// Logging...
memcpy(instPtr->log.rxLog.data, msg, msgLen);
instPtr->log.rxLog.dataId = message->svcDataId;
#ifdef DATALINK_ENABLE_AUTOTIMESYNC
// Implement a auto time sync.
if (L2_GetNodeType(instPtr) != L2_NODE_HEAD)
{
// Time sync.
L2_TimeAdjust(instPtr, &(instPtr->log.timeDev));
}
#endif
return Status;
}
/*****************************************************************************/
/**
* @brief Initialize service net FSM
@@ -583,4 +893,4 @@ int32_t L2_Service_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntr
}
/** @} LinkLayer_Service*/
/** @} LinkLayer*/
/** @} LinkLayer*/
df_datalink_sp/src/networking/phy_layer/phy_layer.c
+2 -2
View File
@@ -64,7 +64,7 @@
/************************** Variable Definitions *****************************/
extern const uint16_t freqTable[L1_FRAME_MAX_HOPS];
extern const int16_t antCalTable[L1_FRAME_MAX_HOPS][L1_ANT_CH_NUM];
extern const int16_t antCalTable[L1_ANT_NUM][L1_FRAME_MAX_HOPS][L1_ANT_CH_NUM];
extern const uint8_t hopPattern_ADV_24MS[L1_FRAME_ADV_24MS_HOPS];
extern const uint8_t hopPattern_ACK_8MS[L1_FRAME_ACK_8MS_HOPS];
extern const uint8_t hopPattern_128KBPS_4MS[L1_FRAME_128KBPS_4MS_HOPS];
@@ -388,7 +388,7 @@ int32_t Phy_Init(PHY_INST **instPtr, PHY_CONFIG *phyConfig)
Phy_AntConfigInit(&((*instPtr)->antConfig));
memset(&((*instPtr))->tor, PHY_TOR_SUSPEND, sizeof(PHY_RCD));
memset(&((*instPtr)->frameType), L1_NONE_FRAME, sizeof(PHY_RCD));
memset(&((*instPtr)->frameType), L1_FRAME_NONE, sizeof(PHY_RCD));
memset(&((*instPtr)->antIndex), (int32_t)L1_INIT_ANT_IDX, sizeof(PHY_RCD));
memset(&((*instPtr)->hwSate), 0x00, sizeof(HWSTATE));
memset(&((*instPtr)->phyLog), 0x00, sizeof(PHYLOG));
df_datalink_sp/src/system_monitor.h
+3 -3
View File
@@ -148,9 +148,9 @@ typedef struct __attribute__((packed))
uint32_t ackTxCnt; ///< ack frame send count
uint32_t ackRecvCnt; ///< ack frame received count
uint32_t ackCorrectCnt; ///< ack frame received correctly count
uint32_t distTxCnt; ///< dist frame send count
uint32_t distRecvCnt; ///< dist frame received count
uint32_t distCorrectCnt;///< dist frame received correctly count
uint32_t disTxCnt; ///< dist frame send count
uint32_t disRecvCnt; ///< dist frame received count
uint32_t disCorrectCnt;///< dist frame received correctly count
uint32_t mngTxCnt; ///< mng frame send count
uint32_t mngRecvCnt; ///< mng frame received count
uint32_t mngCorrectCnt; ///< mng frame received correctly count
df_datalink_sp/src/systembus/systembus.c
+35 -35
View File
@@ -310,40 +310,40 @@ static int32_t SystemBus_SystemConfig(SYSBUS *instPtr, uint8_t *msg)
return EPARAM;
}
if (msg[WD_EN_CMD] == EN_CMD_ON_HIGH_POWER)
{
// full power config
Phy_AntConfigMode(phyRef, PHY_ANT_MODE_POWER_FULL);
if (reconfig || !L2_isStarted(linkInstPtr))
{
Linklayer_Reconfig(linkInstPtr, &linkConfig);
}
Linklayer_Start(linkInstPtr);
}
else if (msg[WD_EN_CMD] == EN_CMD_ON_LOW_POWER)
{
// low power config
Phy_AntConfigMode(phyRef, PHY_ANT_MODE_POWER_LOW);
if (reconfig || !L2_isStarted(linkInstPtr))
{
Linklayer_Reconfig(linkInstPtr, &linkConfig);
}
Linklayer_Start(linkInstPtr);
}
else if (msg[WD_EN_CMD] == EN_CMD_SILENT)
{
// silence config
// TODO: better approach?
Linklayer_Halt(linkInstPtr);
Phy_AntConfigMode(phyRef, PHY_ANT_MODE_STANDBY);
TIME_WORD pullTime = {0, 0};
pullTime.int_part = TimeCtrl_GetIntCnt(tmcRef) + 1;
if (Phy_SetTimeWord(phyRef, &pullTime, PHY_TIME_WORD_POLL) != SUCCESS)
{
DBG_ERROR("Physical set pull time failed.\n");
}
Phy_SetupNextSlot(phyRef, PHY_TOR_RX, L1_10KBPS_10MS_FRAME, NULL);
}
// if (msg[WD_EN_CMD] == EN_CMD_ON_HIGH_POWER)
// {
// // full power config
// Phy_AntConfigMode(phyRef, PHY_ANT_MODE_POWER_FULL);
// if (reconfig || !L2_isStarted(linkInstPtr))
// {
// Linklayer_Reconfig(linkInstPtr, &linkConfig);
// }
// Linklayer_Start(linkInstPtr);
// }
// else if (msg[WD_EN_CMD] == EN_CMD_ON_LOW_POWER)
// {
// // low power config
// Phy_AntConfigMode(phyRef, PHY_ANT_MODE_POWER_LOW);
// if (reconfig || !L2_isStarted(linkInstPtr))
// {
// Linklayer_Reconfig(linkInstPtr, &linkConfig);
// }
// Linklayer_Start(linkInstPtr);
// }
// else if (msg[WD_EN_CMD] == EN_CMD_SILENT)
// {
// // silence config
// // TODO: better approach?
// Linklayer_Halt(linkInstPtr);
// Phy_AntConfigMode(phyRef, PHY_ANT_MODE_STANDBY);
// TIME_WORD pullTime = {0, 0};
// pullTime.int_part = TimeCtrl_GetIntCnt(tmcRef) + 1;
// if (Phy_SetTimeWord(phyRef, &pullTime, PHY_TIME_WORD_POLL) != SUCCESS)
// {
// DBG_ERROR("Physical set pull time failed.\n");
// }
// Phy_SetupNextSlot(phyRef, PHY_TOR_RX, L1_10KBPS_10MS_FRAME, NULL);
// }
else
{
// Ignore current command
@@ -777,4 +777,4 @@ int32_t SystemBus_RxIntrHandler(SYSBUS *instPtr)
return SUCCESS;
}
/** @} SystemBus*/
/** @} SystemBus*/