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); ASSERT(instPtr != NULL);
int32_t Status; 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) switch (type)
{ {
case L2_FRAME_ADV: case L2_FRAME_ADV:
{ L2_MSG_ADV *message = (L2_MSG_ADV *)(instPtr->sendPktData);
L2_MSG_ADV *message = calloc(sizeof(L2_MSG_ADV));
// Common section of link layer package // Common section of link layer package
message->common->frameNum = L2_GetNextFrameNum(instPtr); 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 // insert position and speed info
SysMonitor_GetSysinfo(&(instPtr->sysInfo)); SysMonitor_GetSysinfo(&(instPtr->sysInfo));
message->srcPosX = &(instPtr->sysInfo.posX); message->srcPosX = instPtr->sysInfo.posX;
message->srcPosY = &(instPtr->sysInfo.posY); message->srcPosY = instPtr->sysInfo.posY;
message->srcPosZ = &(instPtr->sysInfo.posZ); message->srcPosZ = instPtr->sysInfo.posZ;
// Insert net start time // Insert net start time
message->srcNetStatTime = &(instPtr->netStartTime); message->srcNetStatTime = &(instPtr->netStartTime);
// Packing extra payload // 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->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 // Append CRC
uint32_t crcVal; 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) if (Status != SUCCESS)
{ {
DBG_ERROR("Fail to generate CRC, error code: %" PRId32 ".\n", Status);
return Status; return Status;
} }
message->crc = crcVal; message->crc = crcVal;
instPtr->sendPktData = message;
// Logging... // Logging...
memcpy(instPtr->log.txLog.data, message, Phy_GetFrameLen(L2_L2frame2L1frame(L2_FRAME_ADV))); memcpy(instPtr->log.txLog.data, message, sizeof(L2_MSG_ADV));
instPtr->log.txLog.dataId = message->ackDataId; instPtr->log.txLog.dataId = message->advDataId;
free(message);
break; break;
}
case L2_FRAME_ACK: case L2_FRAME_ACK:
{ L2_MSG_ACK *message = (L2_MSG_ACK *)(instPtr->sendPktData);
L2_MSG_ACK *message = calloc(sizeof(L2_MSG_ACK));
// Common section of link layer package // Common section of link layer package
message->common->frameNum = L2_GetNextFrameNum(instPtr); 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 // insert position and speed info
SysMonitor_GetSysinfo(&(instPtr->sysInfo)); SysMonitor_GetSysinfo(&(instPtr->sysInfo));
message->srcPosX = &(instPtr->sysInfo.posX); message->srcPosX = instPtr->sysInfo.posX;
message->srcPosY = &(instPtr->sysInfo.posY); message->srcPosY = instPtr->sysInfo.posY;
message->srcPosZ = &(instPtr->sysInfo.posZ); message->srcPosZ = instPtr->sysInfo.posZ;
message->srcSpeedX = &(instPtr->&(instPtr->sysInfo.vX)); message->srcSpeedX = instPtr->sysInfo.vX;
message->srcSpeedY = &(instPtr->&(instPtr->sysInfo.vY)); message->srcSpeedY = instPtr->sysInfo.vY;
message->srcSpeedZ = &(instPtr->&(instPtr->sysInfo.vZ)); message->srcSpeedZ = instPtr->sysInfo.vZ;
// Packing extra payload // Packing extra payload
SysMonitor_GetSendPayload(message->ackData, message->ackDataId, L2_PAYLOAD_LEN_ACK); 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 // Append CRC
uint32_t crcVal; 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) if (Status != SUCCESS)
{ {
DBG_ERROR("Fail to generate CRC, error code: %" PRId32 ".\n", Status);
return Status; return Status;
} }
message->crc = crcVal; message->crc = crcVal;
// Logging... // 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; instPtr->log.txLog.dataId = message->ackDataId;
free(message);
break; break;
}
default: default:
{
DBG_ERROR("Unexpected frame type: %" PRId8 ".\n", type); DBG_ERROR("Unexpected frame type: %" PRId8 ".\n", type);
return EPARAM; return EPARAM;
} }
}
return SUCCESS; return SUCCESS;
} }
@@ -191,7 +177,7 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg)
break; break;
} }
L2_MSG_ADV *message = msg; L2_MSG_ADV *message = (L2_MSG_ADV *)msg;
// Update node status. // Update node status.
uint8_t nodeId = message->common.src; uint8_t nodeId = message->common.src;
@@ -207,11 +193,12 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg)
break; break;
} }
} }
// Update node info. // Update node info.
node->nodeInfo.pos.x = message->srcPosX; node->nodeInfo.pos.x = message->srcPosX;
node->nodeInfo.pos.y = message->srcPosY; node->nodeInfo.pos.y = message->srcPosY;
node->nodeInfo.pos.z = message->srcPosZ; 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 // Reset node ttl
node->ttlCnt = 0; 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); SysMonitor_UpdateRecvPayload(message->advData, dataId, L2_PAYLOAD_LEN_ADV);
// Logging... // 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; instPtr->log.rxLog.dataId = dataId;
Status = SUCCESS; Status = SUCCESS;
@@ -268,7 +255,7 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg)
break; break;
} }
L2_MSG_ACK *message = msg; L2_MSG_ACK *message = (L2_MSG_ACK *)msg;
// Update node status. // Update node status.
uint8_t nodeId = message->common.src; uint8_t nodeId = message->common.src;
@@ -284,6 +271,7 @@ int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg)
break; break;
} }
} }
// Update node info. // Update node info.
node->nodeInfo.pos.x = message->srcPosX; node->nodeInfo.pos.x = message->srcPosX;
node->nodeInfo.pos.y = message->srcPosY; 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.x = message->srcSpeedX;
node->nodeInfo.speed.y = message->srcSpeedY; node->nodeInfo.speed.y = message->srcSpeedY;
node->nodeInfo.speed.z = message->srcSpeedZ; 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 // Reset node ttl
node->ttlCnt = 0; 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); SysMonitor_UpdateRecvPayload(message->advData, dataId, L2_PAYLOAD_LEN_ADV);
// Logging... // 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; instPtr->log.rxLog.dataId = dataId;
Status = SUCCESS; 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) int32_t L2_Scan_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntry)
{ {
ASSERT(instPtr != NULL); 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"); DBG_INFO("-------- Scan Routine --------\n");
if (rtneEntry->slotSeq != 0) 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 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) switch (rtneEntry->slotType)
{ {
case L2_SLOT_ADVERTISE: case L2_SLOT_ADVERTISE:
if (L2_GetNodeType(instPtr) == L2_NODE_HEAD && rtneEntry->src == L2_GetNodeId(instPtr)) 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 // antenna selection based on broadcast id in routing table
torState = L2_TOR_TX; // head node transmits messages in advertise slot 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) if (ScanMsg_Packing(instPtr, L2_FRAME_ADV, rtneEntry->dst) != SUCCESS)
{ {
return FAILURE; 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) else if (L2_GetNodeType(instPtr) == L2_NODE_MEMBER)
{ {
commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->src); commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->src);
antIndex = commNode->nodeInfo.antIndex; // antenna selection based on node list // antenna selection based on node list
torState = L2_TOR_RX; // member node receives message in advertise slot antIndex = commNode->nodeInfo.antIndex;
// member node receives message in advertise slot
torState = L2_TOR_RX;
} }
else else
{ {
DBG_ERROR("Unsupported node type" PRId8 "or wrong routine table in L2_SLOT_ADVERTISE.\n", L2_GetNodeType(instPtr)); // otherwise suspend
return FAILURE; antIndex = PHY_ANT_INDEX_NONE;
torState = L2_TOR_SUSPEND;
} }
(void)Phy_SetAntIndex(instPtr->config.phyInstPtr, antIndex); (void)Phy_SetAntIndex(instPtr->config.phyInstPtr, antIndex);
(void)L2_SetupNextSlot(instPtr, torState, L2_FRAME_ADV); (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: case L2_SLOT_ACKNOWLEDGE:
if (L2_GetNodeType(instPtr) == L2_NODE_HEAD && rtneEntry->dst == L2_GetNodeId(instPtr)) if (L2_GetNodeType(instPtr) == L2_NODE_HEAD && rtneEntry->dst == L2_GetNodeId(instPtr))
{ {
antIndex = PHY_ANT_INDEX_NONE; // FIXME: is antenna index necessary here? // FIXME: is antenna index necessary here?
torState = L2_TOR_RX; // head node receives messages in acknowledge slot 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); commNode = L2_GetNodeById(instPtr->nodeList, rtneEntry->src);
antIndex = commNode->nodeInfo.antIndex; // antenna selection based on node list // antenna selection based on node list
torState = L2_TOR_TX; // member node transmits message in acknowledge slot 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) if (ScanMsg_Packing(instPtr, L2_FRAME_ACK, rtneEntry->dst) != SUCCESS)
{ {
return FAILURE; return FAILURE;
@@ -438,8 +442,9 @@ int32_t L2_Scan_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntry)
} }
else else
{ {
DBG_ERROR("Unsupported node type" PRId8 "or wrong routine table in L2_SLOT_ACKNOWLEDGE.\n", L2_GetNodeType(instPtr)); // otherwise suspend
return FAILURE; antIndex = PHY_ANT_INDEX_NONE;
torState = L2_TOR_SUSPEND;
} }
(void)Phy_SetAntIndex(instPtr->config.phyInstPtr, antIndex); (void)Phy_SetAntIndex(instPtr->config.phyInstPtr, antIndex);
(void)L2_SetupNextSlot(instPtr, torState, L2_FRAME_ACK); (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 * @param instPtr a pointer to physical instance
* *
* @return antenna index * @return antenna index
* *
******************************************************************************/ ******************************************************************************/
uint8_t Phy_ListenAntIndex(PHY_INST *instPtr) uint8_t Phy_ReceiveAntIndex(PHY_INST *instPtr)
{ {
ASSERT(instPtr != NULL); ASSERT(instPtr != NULL);
return (uint8_t)PhyRead(instPtr, ANT_SELECT_INDEX); 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 * @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_GetFreqEst(PHY_INST *instPtr);
double Phy_GetCoarseFreqEst(PHY_INST *instPtr); double Phy_GetCoarseFreqEst(PHY_INST *instPtr);
double Phy_GetFineFreqEst(PHY_INST *instPtr); double Phy_GetFineFreqEst(PHY_INST *instPtr);