0xha0/datalink_repo
2
2
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

miscellaneous update.

Browse Source
This commit is contained in:
Gatsby Van
2023-12-21 14:48:28 +08:00
parent 97b46fb0e8
commit ebe3ebf71d
6 changed files with 519 additions and 519 deletions
Download Patch File Download Diff File Expand all files Collapse all files
df_datalink/src/networking/link_layer/scan.c
+63 -63
View File
@@ -33,7 +33,7 @@
/************************** Variable Definitions *****************************/
/************************** Function Prototypes ******************************/
static int32_t ScanMsg_Packing(L2_INST *l2InstPtr, L2_FRAME_TYPE type);
static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type);
/*****************************************************************************/
/**
* @brief Packing Scan message to a bit array.
@@ -45,35 +45,35 @@ static int32_t ScanMsg_Packing(L2_INST *l2InstPtr, L2_FRAME_TYPE type);
* SUCCESS
*
******************************************************************************/
static int32_t ScanMsg_Packing(L2_INST *l2InstPtr, L2_FRAME_TYPE type) {
ASSERT(l2InstPtr != NULL);
static int32_t ScanMsg_Packing(L2_INST *instPtr, L2_FRAME_TYPE type) {
ASSERT(instPtr != NULL);
int32_t Status;
uint8_t *buff;
uint32_t packLen;
switch (type) {
case L2_INFO_FRAME:{
packLen = RSP_PACK_LEN;
SysMonitor_GetSysinfo(&(l2InstPtr->sysInfo));
SysMonitor_GetSysinfo(&(instPtr->sysInfo));
buff = malloc(packLen);
if (buff == NULL) {
DBG_ERROR("Not enough space, failed to pack frame...\n");
return ENOMEM;
}
buff[L2_MSG_FRAME_NO] = L2_GetNextFrameNo(l2InstPtr);
buff[L2_MSG_SLOT_NO] = L2_GetNextSlotNo(l2InstPtr);
memcpy(buff+L2_MSG_INFO_POSX, &(l2InstPtr->sysInfo.posX), sizeof(float));
memcpy(buff+L2_MSG_INFO_POSY, &(l2InstPtr->sysInfo.posY), sizeof(float));
memcpy(buff+L2_MSG_INFO_POSZ, &(l2InstPtr->sysInfo.posZ), sizeof(float));
memcpy(buff+L2_MSG_INFO_SPEEDX, &(l2InstPtr->sysInfo.vX), sizeof(float));
memcpy(buff+L2_MSG_INFO_SPEEDY, &(l2InstPtr->sysInfo.vY), sizeof(float));
memcpy(buff+L2_MSG_INFO_SPEEDZ, &(l2InstPtr->sysInfo.vZ), sizeof(float));
buff[L2_MSG_FRAME_NO] = L2_GetNextFrameNo(instPtr);
buff[L2_MSG_SLOT_NO] = L2_GetNextSlotNo(instPtr);
memcpy(buff+L2_MSG_INFO_POSX, &(instPtr->sysInfo.posX), sizeof(float));
memcpy(buff+L2_MSG_INFO_POSY, &(instPtr->sysInfo.posY), sizeof(float));
memcpy(buff+L2_MSG_INFO_POSZ, &(instPtr->sysInfo.posZ), sizeof(float));
memcpy(buff+L2_MSG_INFO_SPEEDX, &(instPtr->sysInfo.vX), sizeof(float));
memcpy(buff+L2_MSG_INFO_SPEEDY, &(instPtr->sysInfo.vY), sizeof(float));
memcpy(buff+L2_MSG_INFO_SPEEDZ, &(instPtr->sysInfo.vZ), sizeof(float));
Status = L2_CrcAppend(buff, packLen - 2, L2_MSG_CRC, L2_CRC16_1021);
if (Status != SUCCESS) {
DBG_ERROR("Fail to append CRC, error code: %ld.\n", Status);
return Status;
}
if(CircQueuePush(l2InstPtr->txQueue, buff) == false){
if(CircQueuePush(instPtr->txQueue, buff) == false){
DBG_ERROR("Cannot push packet to queue..\n");
return EQUEUE;
}
@@ -92,15 +92,15 @@ static int32_t ScanMsg_Packing(L2_INST *l2InstPtr, L2_FRAME_TYPE type) {
/**
* @brief Initialize scan FSM.
*
* @param l2InstPtr
* @param instPtr
*
* @return
*
******************************************************************************/
int32_t L2_Scan_Init(L2_INST *l2InstPtr) {
ASSERT(l2InstPtr != NULL);
l2InstPtr->scanFsm.sacanCnt = 0;
l2InstPtr->scanFsm.beamNo = 0;
int32_t L2_Scan_Init(L2_INST *instPtr) {
ASSERT(instPtr != NULL);
instPtr->scanFsm.sacanCnt = 0;
instPtr->scanFsm.beamNo = 0;
return SUCCESS;
}
@@ -113,32 +113,32 @@ int32_t L2_Scan_Init(L2_INST *l2InstPtr) {
* @return
*
******************************************************************************/
int32_t L2_Scan_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) {
ASSERT(l2InstPtr != NULL);
int32_t L2_Scan_PktHandler(L2_INST *instPtr, uint8_t *msg) {
ASSERT(instPtr != NULL);
ASSERT(msg != NULL);
int32_t Status;
switch (l2InstPtr->frame.last) {
switch (instPtr->frame.last) {
case L2_SCAN_FRAME: {
if (l2InstPtr->config.myType == L2_HEAD_NODE) {
if (instPtr->config.myType == L2_HEAD_NODE) {
DBG_ERROR("Unexpected call from head node, Only None-Head Node can handle L2_SVC_FRAME.\n");
Status = EUNEXP;
break;
}
Status = AppendNode(l2InstPtr->nodeInfoList, L2_HEAD_NODE_ID, L2_HEAD_NODE);
Status = AppendNode(instPtr->nodeInfoList, L2_HEAD_NODE_ID, L2_HEAD_NODE);
if (Status != SUCCESS) {
DBG_ERROR("Failed to append node %d, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
}
break;
}
case L2_INFO_FRAME: {
if (l2InstPtr->config.myType != L2_HEAD_NODE) {
if (instPtr->config.myType != L2_HEAD_NODE) {
DBG_ERROR("Unexpected call from member node, Only Head Node can handle L2_SVC_FRAME.\n");
Status = EUNEXP;
break;
}
COORD coordTemp;
float tmpVal;
Status = AppendNode(l2InstPtr->nodeInfoList, L2_MEMBER_NODE_ID, L2_MEMBER_NODE);
Status = AppendNode(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, L2_MEMBER_NODE);
if (Status != SUCCESS) {
DBG_ERROR("Failed to append node %d, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
break;
@@ -149,7 +149,7 @@ int32_t L2_Scan_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) {
coordTemp.y = tmpVal;
memcpy(&tmpVal, msg+L2_MSG_INFO_POSZ, sizeof(float));
coordTemp.z = tmpVal;
Status = UpdateNodePos(l2InstPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp);
Status = UpdateNodePos(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp);
if (Status != SUCCESS) {
DBG_ERROR("Failed to update node %d position, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
}
@@ -159,16 +159,16 @@ int32_t L2_Scan_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) {
coordTemp.y = tmpVal;
memcpy(&tmpVal, msg+L2_MSG_INFO_SPEEDZ, sizeof(float));
coordTemp.z = tmpVal;
Status = UpdateNodeSpeed(l2InstPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp);
Status = UpdateNodeSpeed(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, &coordTemp);
if (Status != SUCCESS) {
DBG_ERROR("Failed to update node %d speed, errorCode: %ld.\n", L2_MEMBER_NODE_ID, Status);
}
// Update Time deviation
int64_t devi, deviTrunc;
Status = L2_TimeSync_GetDeviatoion(l2InstPtr, &devi);
Status = L2_TimeSync_GetDeviatoion(instPtr, &devi);
if (Status != SUCCESS) {
DBG_ERROR("Failed to compute slot deviation.\n");
UpdateNodeTimeDeviation(l2InstPtr->nodeInfoList, L2_MEMBER_NODE_ID, 0);
UpdateNodeTimeDeviation(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, 0);
}
else {
if(devi > INT32_MAX || devi < INT32_MIN) {
@@ -182,25 +182,25 @@ int32_t L2_Scan_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) {
else {
deviTrunc = (int32_t)devi;
}
UpdateNodeTimeDeviation(l2InstPtr->nodeInfoList, L2_MEMBER_NODE_ID, (uint32_t)deviTrunc);
UpdateNodeTimeDeviation(instPtr->nodeInfoList, L2_MEMBER_NODE_ID, (uint32_t)deviTrunc);
}
break;
}
default:
DBG_ERROR("Unsupported frame type: %ld.\n", l2InstPtr->frame.last);
DBG_ERROR("Unsupported frame type: %ld.\n", instPtr->frame.last);
Status = EUNEXP;
break;
}
// Net setup finished.
if(L2_GetMainState(l2InstPtr) == INIT_STATE && Status == SUCCESS){
if(L2_GetMainState(instPtr) == INIT_STATE && Status == SUCCESS){
L2_EVENT l2Event;
L2_RTNE_CONFIG newRtneConfig;
// Assert Net-Setup Done.
l2Event.event = L2_EV_NET_SETUP_DONE;
l2Event.msg = NULL;
AssertL2Event(l2InstPtr, &l2Event);
AssertL2Event(instPtr, &l2Event);
// Assert Routine Table Update...
newRtneConfig.rtneTable = normalTable;
@@ -208,7 +208,7 @@ int32_t L2_Scan_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) {
newRtneConfig.rtneStep = 0;
l2Event.event = L2_ROUTINE_UPDATE;
l2Event.msg = &newRtneConfig;
AssertL2Event(l2InstPtr, &l2Event);
AssertL2Event(instPtr, &l2Event);
}
return Status;
}
@@ -217,24 +217,24 @@ int32_t L2_Scan_PktHandler(L2_INST *l2InstPtr, uint8_t *msg) {
/**
* @brief Execute a member node scan net procedure.
*
* @param l2InstPtr A pointer to scan net fsm.
* @param instPtr A pointer to scan net fsm.
*
* @return used slot (5ms slot).
*
******************************************************************************/
int32_t L2_Scan_Proc(L2_INST *l2InstPtr) {
int32_t L2_Scan_Proc(L2_INST *instPtr) {
int32_t Status;
/* l2InstPtr->nextState == NORMAL_STATE indicates network set up procedure was finished.
/* instPtr->nextState == NORMAL_STATE indicates network set up procedure was finished.
* Skip 5ms for nodes for node configuration update.
*/
DBG_INFO("-------- Scan Process --------\n");
if (l2InstPtr->nextState == NORMAL_STATE) {
if (instPtr->nextState == NORMAL_STATE) {
DBG_INFO("NETSET DONE, halt 20ms before normal routine.\n");
if(l2InstPtr->config.myType != L2_HEAD_NODE) {
Phy_SetBeamAngle(l2InstPtr->config.phyInstRef, 0.0, 0.0);
Phy_SetBeamWidth(l2InstPtr->config.phyInstRef, PHY_ANT_BEAM_120);
Phy_SetMode(l2InstPtr->config.phyInstRef, PHY_MODE_SCHEDULE);
L2_SetupNextSlot(l2InstPtr, L2_TOR_SUSPEND, L2_SCAN_FRAME);
if(instPtr->config.myType != L2_HEAD_NODE) {
Phy_SetBeamAngle(instPtr->config.phyInstRef, 0.0, 0.0);
Phy_SetBeamWidth(instPtr->config.phyInstRef, PHY_ANT_BEAM_120);
Phy_SetMode(instPtr->config.phyInstRef, PHY_MODE_SCHEDULE);
L2_SetupNextSlot(instPtr, L2_TOR_SUSPEND, L2_SCAN_FRAME);
}
else {
DBG_ERROR("Illegal call from Head Node.\n");
@@ -249,23 +249,23 @@ int32_t L2_Scan_Proc(L2_INST *l2InstPtr) {
* Increase beam number after a Tx-Rx procedure pair, reset beam number when
* it reaches the boundary.
*/
if (l2InstPtr->config.myType == L2_HEAD_NODE) {
if (instPtr->config.myType == L2_HEAD_NODE) {
// Apply a scan procedure
if (l2InstPtr->scanFsm.sacanCnt % 2 == 0){
if (instPtr->scanFsm.sacanCnt % 2 == 0){
// Antenna configure
Phy_SetBeamWidth(l2InstPtr->config.phyInstRef, PHY_ANT_BEAM_8);
Phy_SetBeamNo(l2InstPtr->config.phyInstRef, l2InstPtr->scanFsm.beamNo);
Phy_SetMode(l2InstPtr->config.phyInstRef, PHY_MODE_SCHEDULE);
L2_SetupNextSlot(l2InstPtr, L2_TOR_TX, L2_SCAN_FRAME);
Phy_SetBeamWidth(instPtr->config.phyInstRef, PHY_ANT_BEAM_8);
Phy_SetBeamNo(instPtr->config.phyInstRef, instPtr->scanFsm.beamNo);
Phy_SetMode(instPtr->config.phyInstRef, PHY_MODE_SCHEDULE);
L2_SetupNextSlot(instPtr, L2_TOR_TX, L2_SCAN_FRAME);
}
else {
// Antenna configure
Phy_SetBeamWidth(l2InstPtr->config.phyInstRef, PHY_ANT_BEAM_8);
Phy_SetBeamNo(l2InstPtr->config.phyInstRef, l2InstPtr->scanFsm.beamNo);
Phy_SetMode(l2InstPtr->config.phyInstRef, PHY_MODE_SCHEDULE);
L2_SetupNextSlot(l2InstPtr, L2_TOR_RX, L2_INFO_FRAME);
Phy_SetBeamWidth(instPtr->config.phyInstRef, PHY_ANT_BEAM_8);
Phy_SetBeamNo(instPtr->config.phyInstRef, instPtr->scanFsm.beamNo);
Phy_SetMode(instPtr->config.phyInstRef, PHY_MODE_SCHEDULE);
L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INFO_FRAME);
// Update Scan beam number
l2InstPtr->scanFsm.beamNo = (l2InstPtr->scanFsm.beamNo + 1) % L1_NUM_ANT_NRRW_PATTERN;
instPtr->scanFsm.beamNo = (instPtr->scanFsm.beamNo + 1) % L1_NUM_ANT_NRRW_PATTERN;
}
}
/* Member node listening the head node pilot signal with wide beam width.
@@ -278,26 +278,26 @@ int32_t L2_Scan_Proc(L2_INST *l2InstPtr) {
else {
uint8_t *sendData;
// Pack node information.
Status = ScanMsg_Packing(l2InstPtr, L2_INFO_FRAME);
Status = ScanMsg_Packing(instPtr, L2_INFO_FRAME);
if (Status != SUCCESS) {
DBG_INFO("Failed to pack response.\n");
}
if (CircQueuePop(l2InstPtr->txQueue, (void **)(&sendData)) == true) {
memcpy(l2InstPtr->sendPktData, sendData, RSP_PACK_LEN);
if (CircQueuePop(instPtr->txQueue, (void **)(&sendData)) == true) {
memcpy(instPtr->sendPktData, sendData, RSP_PACK_LEN);
free(sendData);
}
else {
DBG_INFO("Failed to pop tx Queue.\n");
}
// Antenna configure
Phy_SetBeamAngle(l2InstPtr->config.phyInstRef, 0.0, 0.0);
Phy_SetBeamWidth(l2InstPtr->config.phyInstRef, PHY_ANT_BEAM_120);
Phy_SetMode(l2InstPtr->config.phyInstRef, PHY_MODE_LISTEN);
L2_SetupNextSlot(l2InstPtr, L2_TOR_RX, L2_SCAN_FRAME);
Phy_SetBeamAngle(instPtr->config.phyInstRef, 0.0, 0.0);
Phy_SetBeamWidth(instPtr->config.phyInstRef, PHY_ANT_BEAM_120);
Phy_SetMode(instPtr->config.phyInstRef, PHY_MODE_LISTEN);
L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_SCAN_FRAME);
}
// Increase scan counter (for debug purpose).
l2InstPtr->scanFsm.sacanCnt ++;
instPtr->scanFsm.sacanCnt ++;
return NUM_SLOTS_SKIP;
}
@@ -311,7 +311,7 @@ int32_t L2_Scan_Proc(L2_INST *l2InstPtr) {
* @return used slot (5ms slot).
*
******************************************************************************/
int32_t L2_Scan_Routine(L2_INST *l2InstPtr, L2_RTNE_TABLE_ENTRY *schedEntry) {
int32_t L2_Scan_Routine(L2_INST *instPtr, L2_RTNE_TABLE_ENTRY *schedEntry) {
DBG_INFO("-------- Scan Routine --------\n");
ASSERT(false); // Not support.
return 0;