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

fix: 修正建网数据打包, 增加routine处理分支

Browse Source
This commit is contained in:
josh
2025-05-21 17:11:38 +08:00
parent 6d665eb853
commit 169cb6bd91
3 changed files with 65 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files
df_datalink_sp/src/networking/link_layer/scan.c
+62 -57
View File
@@ -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);
df_datalink_sp/src/networking/phy_layer/phy_layer.c
+2 -2
View File
@@ -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);
df_datalink_sp/src/networking/phy_layer/phy_layer.h
+1 -1
View File
@@ -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);